diff --git a/.github/labeler.yml b/.github/labeler.yml new file mode 100644 index 00000000000..c6869bb3c4c --- /dev/null +++ b/.github/labeler.yml @@ -0,0 +1,77 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Documentation-related changes +documentation: + - all: + - changed-files: + - any-glob-to-any-file: + - 'docs/**' + - '**/README.md' + - all-globs-to-all-files: + - '!docs/licenses/**' + +# Infrastructure changes +infrastructure: + - changed-files: + - any-glob-to-any-file: + - .github/** + - docker/** + - .dockerignore + - tools/** + - .vscode/** + - environment.yml + - setup.py + - pyproject.toml + - .pre-commit-config.yaml + - .flake8 + - isaaclab.sh + - isaaclab.bat + - docs/licenses/** + +# Assets (USD, glTF, etc.) related changes. +asset: + - changed-files: + - any-glob-to-any-file: + - source/isaaclab_assets/** + +# Isaac Sim team related changes. +isaac-sim: + - changed-files: + - any-glob-to-any-file: + - apps/** + +# Isaac Mimic team related changes. +isaac-mimic: + - changed-files: + - any-glob-to-any-file: + - source/isaaclab/isaaclab/devices/** + - source/isaaclab_mimic/** + - source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack** + - source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_and_place** + - scripts/imitation_learning/** + +# Isaac Lab team related changes. +isaac-lab: + - all: + - changed-files: + - any-glob-to-any-file: + - source/** + - scripts/** + - all-globs-to-all-files: + - '!source/isaaclab_assets/**' + - '!source/isaaclab_mimic/**' + - '!source/isaaclab/isaaclab/devices' + - '!scripts/imitation_learning/**' + +# Add 'enhancement' label to any PR where the head branch name +# starts with `feature` or has a `feature` section in the name +enhancement: + - head-branch: ['^feature', 'feature'] + +# Add 'bug' label to any PR where the head branch name +# starts with `fix`/`bug` or has a `fix`/`bug` section in the name +bug: + - head-branch: ['^fix', 'fix', '^bug', 'bug'] diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 89c6501a2ee..c25fb650fcd 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -10,6 +10,7 @@ on: branches: - devel - main + - 'release/**' # Concurrency control to prevent parallel runs on the same PR concurrency: @@ -24,8 +25,8 @@ permissions: env: NGC_API_KEY: ${{ secrets.NGC_API_KEY }} - ISAACSIM_BASE_IMAGE: ${{ vars.ISAACSIM_BASE_IMAGE || 'nvcr.io/nvidia/isaac-sim' }} - ISAACSIM_BASE_VERSION: ${{ vars.ISAACSIM_BASE_VERSION || '5.0.0' }} + ISAACSIM_BASE_IMAGE: 'nvcr.io/nvidian/isaac-sim' + ISAACSIM_BASE_VERSION: 'latest-release-5-1' DOCKER_IMAGE_TAG: isaac-lab-dev:${{ github.event_name == 'pull_request' && format('pr-{0}', github.event.pull_request.number) || github.ref_name }}-${{ github.sha }} jobs: @@ -75,6 +76,20 @@ jobs: retention-days: 1 compression-level: 9 + - name: Check Test Results for Fork PRs + if: github.event.pull_request.head.repo.full_name != github.repository + run: | + if [ -f "reports/isaaclab-tasks-report.xml" ]; then + # Check if the test results contain any failures + if grep -q 'failures="[1-9]' reports/isaaclab-tasks-report.xml || grep -q 'errors="[1-9]' reports/isaaclab-tasks-report.xml; then + echo "Tests failed for PR from fork. The test report is in the logs. Failing the job." + exit 1 + fi + else + echo "No test results file found. This might indicate test execution failed." + exit 1 + fi + test-general: runs-on: [self-hosted, gpu] timeout-minutes: 180 @@ -121,11 +136,19 @@ jobs: retention-days: 1 compression-level: 9 - - name: Fail on Test Failure for Fork PRs - if: github.event.pull_request.head.repo.full_name != github.repository && steps.run-general-tests.outcome == 'failure' + - name: Check Test Results for Fork PRs + if: github.event.pull_request.head.repo.full_name != github.repository run: | - echo "Tests failed for PR from fork. The test report is in the logs. Failing the job." - exit 1 + if [ -f "reports/general-tests-report.xml" ]; then + # Check if the test results contain any failures + if grep -q 'failures="[1-9]' reports/general-tests-report.xml || grep -q 'errors="[1-9]' reports/general-tests-report.xml; then + echo "Tests failed for PR from fork. The test report is in the logs. Failing the job." + exit 1 + fi + else + echo "No test results file found. This might indicate test execution failed." + exit 1 + fi combine-results: needs: [test-isaaclab-tasks, test-general] diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index 36ceeffa834..08bf3d2a8bf 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -10,6 +10,7 @@ on: branches: - main - devel + - 'release/**' pull_request: types: [opened, synchronize, reopened] @@ -27,8 +28,7 @@ jobs: - id: trigger-deploy env: REPO_NAME: ${{ secrets.REPO_NAME }} - BRANCH_REF: ${{ secrets.BRANCH_REF }} - if: "${{ github.repository == env.REPO_NAME && github.ref == env.BRANCH_REF }}" + if: "${{ github.repository == env.REPO_NAME && (github.ref == 'refs/heads/main' || github.ref == 'refs/heads/devel' || startsWith(github.ref, 'refs/heads/release/')) }}" run: echo "defined=true" >> "$GITHUB_OUTPUT" build-docs: diff --git a/.github/workflows/labeler.yml b/.github/workflows/labeler.yml new file mode 100644 index 00000000000..8b06dc14407 --- /dev/null +++ b/.github/workflows/labeler.yml @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +name: "Pull Request Labeler" +on: +- pull_request_target + +jobs: + labeler: + permissions: + contents: read + pull-requests: write + runs-on: ubuntu-latest + steps: + - uses: actions/labeler@v6 diff --git a/.github/workflows/license-check.yaml b/.github/workflows/license-check.yaml index 3e7b190cbac..6260199e1dc 100644 --- a/.github/workflows/license-check.yaml +++ b/.github/workflows/license-check.yaml @@ -24,16 +24,20 @@ jobs: # - name: Install jq # run: sudo apt-get update && sudo apt-get install -y jq + - name: Clean up disk space + run: | + rm -rf /opt/hostedtoolcache + - name: Set up Python uses: actions/setup-python@v4 with: - python-version: '3.10' # Adjust as needed + python-version: '3.11' # Adjust as needed - name: Install dependencies using ./isaaclab.sh -i run: | # first install isaac sim pip install --upgrade pip - pip install 'isaacsim[all,extscache]==4.5.0' --extra-index-url https://pypi.nvidia.com + pip install 'isaacsim[all,extscache]==${{ vars.ISAACSIM_BASE_VERSION || '5.0.0' }}' --extra-index-url https://pypi.nvidia.com chmod +x ./isaaclab.sh # Make sure the script is executable # install all lab dependencies ./isaaclab.sh -i @@ -48,6 +52,12 @@ jobs: - name: Print License Report run: pip-licenses --from=mixed --format=markdown + # Print pipdeptree + - name: Print pipdeptree + run: | + pip install pipdeptree + pipdeptree + - name: Check licenses against whitelist and exceptions run: | # Define the whitelist of allowed licenses @@ -118,9 +128,3 @@ jobs: else echo "All packages were checked." fi - - # Print pipdeptree - - name: Print pipdeptree - run: | - pip install pipdeptree - pipdeptree diff --git a/.github/workflows/license-exceptions.json b/.github/workflows/license-exceptions.json index 66530033efa..4e35db15646 100644 --- a/.github/workflows/license-exceptions.json +++ b/.github/workflows/license-exceptions.json @@ -195,13 +195,13 @@ }, { "package": "cmeel-boost", - "license": "UNKNOWN", + "license": "BSL-1.0", "comment": "BSL" }, { "package": "cmeel-console-bridge", - "license": "UNKNOWN", - "comment": "BSD" + "license": "Zlib", + "comment": "ZLIBL" }, { "package": "cmeel-octomap", @@ -215,7 +215,7 @@ }, { "package": "cmeel-tinyxml", - "license": "UNKNOWN", + "license": "Zlib", "comment": "ZLIBL" }, { @@ -225,7 +225,7 @@ }, { "package": "cmeel-zlib", - "license": "UNKNOWN", + "license": "Zlib", "comment": "ZLIBL" }, { @@ -293,7 +293,7 @@ }, { "package": "filelock", - "license": "The Unlicense (Unlicense)", + "license": "Unlicense", "comment": "no condition" }, { @@ -308,7 +308,7 @@ }, { "package": "typing_extensions", - "license": "UNKNOWN", + "license": "Python Software Foundation License", "comment": "PSFL / OSRB" }, { @@ -400,5 +400,45 @@ "package": "fsspec", "license" : "UNKNOWN", "comment": "BSD" + }, + { + "package": "numpy-quaternion", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "aiohappyeyeballs", + "license": "Other/Proprietary License; Python Software Foundation License", + "comment": "PSFL / OSRB" + }, + { + "package": "cffi", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "trio", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "pipdeptree", + "license": "UNKNOWN", + "comment": "MIT" + }, + { + "package": "msgpack", + "license": "UNKNOWN", + "comment": "Apache 2.0" + }, + { + "package": "onnx-ir", + "license": "UNKNOWN", + "comment": "Apache 2.0" + }, + { + "package": "matplotlib-inline", + "license": "UNKNOWN", + "comment": "BSD-3" } ] diff --git a/.github/workflows/postmerge-ci.yml b/.github/workflows/postmerge-ci.yml index 182bc49940c..89bc51b91fa 100644 --- a/.github/workflows/postmerge-ci.yml +++ b/.github/workflows/postmerge-ci.yml @@ -10,6 +10,7 @@ on: branches: - main - devel + - release/** # Concurrency control to prevent parallel runs concurrency: @@ -21,8 +22,8 @@ permissions: env: NGC_API_KEY: ${{ secrets.NGC_API_KEY }} - ISAACSIM_BASE_IMAGE: ${{ vars.ISAACSIM_BASE_IMAGE || 'nvcr.io/nvidia/isaac-sim' }} - ISAACSIM_BASE_VERSIONS_STRING: ${{ vars.ISAACSIM_BASE_VERSIONS_STRING || 'latest-base-5.0' }} + ISAACSIM_BASE_IMAGE: 'nvcr.io/nvidian/isaac-sim' + ISAACSIM_BASE_VERSIONS_STRING: 'latest-release-5-1' ISAACLAB_IMAGE_NAME: ${{ vars.ISAACLAB_IMAGE_NAME || 'isaac-lab-base' }} jobs: @@ -43,8 +44,17 @@ jobs: fetch-depth: 0 lfs: true + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 + with: + platforms: linux/arm64 + - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 + with: + platforms: linux/amd64,linux/arm64 + driver-opts: | + image=moby/buildkit:buildx-stable-1 - name: Login to NGC run: | @@ -100,12 +110,48 @@ jobs: COMBINED_TAG="${REPO_SHORT_NAME}-${SAFE_BRANCH_NAME}-${IMAGE_BASE_VERSION}" BUILD_TAG="${COMBINED_TAG}-b${{ github.run_number }}" + # Determine if multiarch is supported by inspecting the base image manifest + echo "Checking if base image supports multiarch..." + BASE_IMAGE_FULL="${{ env.ISAACSIM_BASE_IMAGE }}:${IMAGE_BASE_VERSION}" + + # Get architectures from the base image manifest + ARCHITECTURES=$(docker manifest inspect "$BASE_IMAGE_FULL" 2>/dev/null | grep -o '"architecture": "[^"]*"' | cut -d'"' -f4 | sort -u) + + if [ -z "$ARCHITECTURES" ]; then + echo "Could not inspect base image manifest: $BASE_IMAGE_FULL" + echo "Defaulting to AMD64 only for safety" + BUILD_PLATFORMS="linux/amd64" + else + echo "Base image architectures found:" + echo "$ARCHITECTURES" | sed 's/^/ - /' + + # Check if both amd64 and arm64 are present + HAS_AMD64=$(echo "$ARCHITECTURES" | grep -c "amd64" || true) + HAS_ARM64=$(echo "$ARCHITECTURES" | grep -c "arm64" || true) + + if [ "$HAS_AMD64" -gt 0 ] && [ "$HAS_ARM64" -gt 0 ]; then + echo "Base image supports multiarch (amd64 + arm64)" + BUILD_PLATFORMS="linux/amd64,linux/arm64" + elif [ "$HAS_AMD64" -gt 0 ]; then + echo "Base image only supports amd64" + BUILD_PLATFORMS="linux/amd64" + elif [ "$HAS_ARM64" -gt 0 ]; then + echo "Base image only supports arm64" + BUILD_PLATFORMS="linux/arm64" + else + echo "Unknown architecture support, defaulting to amd64" + BUILD_PLATFORMS="linux/amd64" + fi + fi + echo "Building image: ${{ env.ISAACLAB_IMAGE_NAME }}:$COMBINED_TAG" echo "IsaacSim version: $IMAGE_BASE_VERSION" + echo "Base image: $BASE_IMAGE_FULL" + echo "Target platforms: $BUILD_PLATFORMS" - # Build Docker image once with both tags + # Build Docker image once with both tags for multiple architectures docker buildx build \ - --platform linux/amd64 \ + --platform $BUILD_PLATFORMS \ --progress=plain \ -t ${{ env.ISAACLAB_IMAGE_NAME }}:$COMBINED_TAG \ -t ${{ env.ISAACLAB_IMAGE_NAME }}:$BUILD_TAG \ @@ -119,6 +165,6 @@ jobs: -f docker/Dockerfile.base \ --push . - echo "✅ Successfully built and pushed: ${{ env.ISAACLAB_IMAGE_NAME }}:$COMBINED_TAG" - echo "✅ Successfully built and pushed: ${{ env.ISAACLAB_IMAGE_NAME }}:$BUILD_TAG" + echo "✅ Successfully built and pushed: ${{ env.ISAACLAB_IMAGE_NAME }}:$COMBINED_TAG (platforms: $BUILD_PLATFORMS)" + echo "✅ Successfully built and pushed: ${{ env.ISAACLAB_IMAGE_NAME }}:$BUILD_TAG (platforms: $BUILD_PLATFORMS)" done diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index f59d4ab7463..05e0d7d60af 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -7,8 +7,7 @@ name: Run linters using pre-commit on: pull_request: - push: - branches: [main] + types: [opened, synchronize, reopened] jobs: pre-commit: diff --git a/.gitignore b/.gitignore index b6c57b6313c..3c4d290429d 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,7 @@ **/.thumbs # No USD files allowed in the repo -**/*.usd + **/*.usda **/*.usdc **/*.usdz @@ -62,7 +62,7 @@ _build /.pretrained_checkpoints/ # Teleop Recorded Dataset -datasets +/datasets/ # Tests tests/ diff --git a/CITATION.cff b/CITATION.cff index ce2eaabc505..71b49b901a3 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -1,7 +1,7 @@ cff-version: 1.2.0 message: "If you use this software, please cite both the Isaac Lab repository and the Orbit paper." title: Isaac Lab -version: 2.2.1 +version: 2.3.0 repository-code: https://github.com/NVIDIA-Omniverse/IsaacLab type: software authors: diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index ee6200de869..145f4db7a7c 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -68,6 +68,7 @@ Guidelines for modifications: * Gary Lvov * Giulio Romualdi * Haoran Zhou +* Harsh Patel * HoJin Jeon * Hongwei Xiong * Hongyu Li @@ -101,6 +102,7 @@ Guidelines for modifications: * Miguel Alonso Jr * Mingyu Lee * Muhong Guo +* Narendra Dahile * Neel Anand Jawale * Nicola Loi * Norbert Cygiert @@ -111,6 +113,7 @@ Guidelines for modifications: * Özhan Özen * Patrick Yin * Peter Du +* Philipp Reist * Pulkit Goyal * Qian Wan * Qinxi Yu @@ -120,6 +123,7 @@ Guidelines for modifications: * Ritvik Singh * Rosario Scalise * Ryley McCarroll +* Sergey Grizan * Shafeef Omar * Shaoshu Su * Shaurya Dewan @@ -127,12 +131,14 @@ Guidelines for modifications: * Shundo Kishi * Stefan Van de Mosselaer * Stephan Pleines +* Tiffany Chen * Tyler Lum * Victor Khaustov * Virgilio Gómez Lambo * Vladimir Fokow * Wei Yang * Xavier Nal +* Xinjie Yao * Xinpeng Liu * Yang Jin * Yanzi Zhu @@ -140,6 +146,7 @@ Guidelines for modifications: * Yohan Choi * Yujian Zhang * Yun Liu +* Zehao Wang * Ziqi Fan * Zoe McCarthy * David Leon diff --git a/README.md b/README.md index bd176eef6b2..020d935ad10 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ # Isaac Lab -[![IsaacSim](https://img.shields.io/badge/IsaacSim-5.0.0-silver.svg)](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html) +[![IsaacSim](https://img.shields.io/badge/IsaacSim-5.1.0-silver.svg)](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html) [![Python](https://img.shields.io/badge/python-3.11-blue.svg)](https://docs.python.org/3/whatsnew/3.11.html) [![Linux platform](https://img.shields.io/badge/platform-linux--64-orange.svg)](https://releases.ubuntu.com/22.04/) [![Windows platform](https://img.shields.io/badge/platform-windows--64-orange.svg)](https://www.microsoft.com/en-us/) @@ -14,6 +14,11 @@ [![License](https://img.shields.io/badge/license-Apache--2.0-yellow.svg)](https://opensource.org/license/apache-2-0) +Please note that this branch is currently under active development in preparation for the Isaac Lab 2.3.0 release. +Some features may not be fully functional while we continue development. +Additional features may depend on unreleased Isaac Sim features that will be available in Isaac Sim 5.1.0. + + **Isaac Lab** is a GPU-accelerated, open-source framework designed to unify and simplify robotics research workflows, such as reinforcement learning, imitation learning, and motion planning. Built on [NVIDIA Isaac Sim](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html), it combines fast and accurate physics and sensor simulation, making it an ideal choice for sim-to-real @@ -37,92 +42,6 @@ Isaac Lab offers a comprehensive set of tools and environments designed to facil ## Getting Started -### Getting Started with Open-Source Isaac Sim - -Isaac Sim is now open source and available on GitHub! - -For detailed Isaac Sim installation instructions, please refer to -[Isaac Sim README](https://github.com/isaac-sim/IsaacSim?tab=readme-ov-file#quick-start). - -1. Clone Isaac Sim - - ``` - git clone https://github.com/isaac-sim/IsaacSim.git - ``` - -2. Build Isaac Sim - - ``` - cd IsaacSim - ./build.sh - ``` - - On Windows, please use `build.bat` instead. - -3. Clone Isaac Lab - - ``` - cd .. - git clone https://github.com/isaac-sim/IsaacLab.git - cd isaaclab - ``` - -4. Set up symlink in Isaac Lab - - Linux: - - ``` - ln -s ../IsaacSim/_build/linux-x86_64/release _isaac_sim - ``` - - Windows: - - ``` - mklink /D _isaac_sim ..\IsaacSim\_build\windows-x86_64\release - ``` - -5. Install Isaac Lab - - Linux: - - ``` - ./isaaclab.sh -i - ``` - - Windows: - - ``` - isaaclab.bat -i - ``` - -6. [Optional] Set up a virtual python environment (e.g. for Conda) - - Linux: - - ``` - source _isaac_sim/setup_conda_env.sh - ``` - - Windows: - - ``` - _isaac_sim\setup_python_env.bat - ``` - -7. Train! - - Linux: - - ``` - ./isaaclab.sh -p scripts/reinforcement_learning/skrl/train.py --task Isaac-Ant-v0 --headless - ``` - - Windows: - - ``` - isaaclab.bat -p scripts\reinforcement_learning\skrl\train.py --task Isaac-Ant-v0 --headless - ``` - ### Documentation Our [documentation page](https://isaac-sim.github.io/IsaacLab) provides everything you need to get started, including @@ -140,12 +59,13 @@ Isaac Lab is built on top of Isaac Sim and requires specific versions of Isaac S release of Isaac Lab. Below, we outline the recent Isaac Lab releases and GitHub branches and their corresponding dependency versions for Isaac Sim. -| Isaac Lab Version | Isaac Sim Version | -| ----------------------------- | ------------------- | -| `main` branch | Isaac Sim 4.5 / 5.0 | -| `v2.2.X` | Isaac Sim 4.5 / 5.0 | -| `v2.1.X` | Isaac Sim 4.5 | -| `v2.0.X` | Isaac Sim 4.5 | +| Isaac Lab Version | Isaac Sim Version | +| ----------------------------- | ------------------------- | +| `main` branch | Isaac Sim 4.5 / 5.0 | +| `v2.3.X` | Isaac Sim 4.5 / 5.0 / 5.1 | +| `v2.2.X` | Isaac Sim 4.5 / 5.0 | +| `v2.1.X` | Isaac Sim 4.5 | +| `v2.0.X` | Isaac Sim 4.5 | ## Contributing to Isaac Lab diff --git a/Uninstall NAO meshes.desktop b/Uninstall NAO meshes.desktop new file mode 100755 index 00000000000..86849c96327 --- /dev/null +++ b/Uninstall NAO meshes.desktop @@ -0,0 +1,9 @@ +[Desktop Entry] +Type=Application +Version=0.9.4 +Name=Uninstall NAO meshes +Comment=Uninstall +Icon= +Exec=/home/lscaglioni/IsaacLab-nomadz/uninstall +Terminal=false +Path=/home/lscaglioni/IsaacLab-nomadz \ No newline at end of file diff --git a/VERSION b/VERSION index c043eea7767..276cbf9e285 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.2.1 +2.3.0 diff --git a/apps/isaaclab.python.headless.kit b/apps/isaaclab.python.headless.kit index 9d3bd66f722..5e93d229c04 100644 --- a/apps/isaaclab.python.headless.kit +++ b/apps/isaaclab.python.headless.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python Headless" description = "An app for running Isaac Lab headlessly" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "headless"] @@ -15,7 +15,7 @@ keywords = ["experience", "app", "isaaclab", "python", "headless"] app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "5.0.0" +app.version = "5.1.0" ################################## # Omniverse related dependencies # @@ -108,7 +108,7 @@ metricsAssembler.changeListenerEnabled = false ############################### [settings.exts."omni.kit.registry.nucleus"] registries = [ - { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/shared" }, + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/107/shared" }, { name = "kit/sdk", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/sdk/${kit_version_short}/${kit_git_hash}" }, { name = "kit/community", url = "https://dw290v42wisod.cloudfront.net/exts/kit/community" }, ] @@ -215,6 +215,6 @@ enabled=true # Enable this for DLSS # set the S3 directory manually to the latest published S3 # note: this is done to ensure prior versions of Isaac Sim still use the latest assets [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaaclab.python.headless.rendering.kit b/apps/isaaclab.python.headless.rendering.kit index dad5e35b40e..b37f33999bf 100644 --- a/apps/isaaclab.python.headless.rendering.kit +++ b/apps/isaaclab.python.headless.rendering.kit @@ -9,7 +9,7 @@ [package] title = "Isaac Lab Python Headless Camera" description = "An app for running Isaac Lab headlessly with rendering enabled" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] @@ -32,7 +32,7 @@ cameras_enabled = true app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "5.0.0" +app.version = "5.1.0" ### FSD app.useFabricSceneDelegate = true @@ -105,7 +105,7 @@ metricsAssembler.changeListenerEnabled = false [settings.exts."omni.kit.registry.nucleus"] registries = [ - { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/shared" }, + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/107/shared" }, { name = "kit/sdk", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/sdk/${kit_version_short}/${kit_git_hash}" }, { name = "kit/community", url = "https://dw290v42wisod.cloudfront.net/exts/kit/community" }, ] @@ -156,6 +156,6 @@ folders = [ # set the S3 directory manually to the latest published S3 # note: this is done to ensure prior versions of Isaac Sim still use the latest assets [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaaclab.python.kit b/apps/isaaclab.python.kit index 9d1687204a3..04c996aa98f 100644 --- a/apps/isaaclab.python.kit +++ b/apps/isaaclab.python.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python" description = "An app for running Isaac Lab" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] @@ -161,7 +161,7 @@ show_menu_titles = true [settings.app] name = "Isaac-Sim" -version = "5.0.0" +version = "5.1.0" versionFile = "${exe-path}/VERSION" content.emptyStageOnStart = true fastShutdown = true @@ -255,7 +255,7 @@ outDirectory = "${data}" ############################### [settings.exts."omni.kit.registry.nucleus"] registries = [ - { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/shared" }, + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/107/shared" }, { name = "kit/sdk", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/sdk/${kit_version_short}/${kit_git_hash}" }, { name = "kit/community", url = "https://dw290v42wisod.cloudfront.net/exts/kit/community" }, ] @@ -302,6 +302,6 @@ fabricUseGPUInterop = true # set the S3 directory manually to the latest published S3 # note: this is done to ensure prior versions of Isaac Sim still use the latest assets [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaaclab.python.rendering.kit b/apps/isaaclab.python.rendering.kit index ab88e1cf905..73c181a0d68 100644 --- a/apps/isaaclab.python.rendering.kit +++ b/apps/isaaclab.python.rendering.kit @@ -9,7 +9,7 @@ [package] title = "Isaac Lab Python Camera" description = "An app for running Isaac Lab with rendering enabled" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] @@ -33,7 +33,7 @@ cameras_enabled = true app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "5.0.0" +app.version = "5.1.0" ### FSD app.useFabricSceneDelegate = true @@ -109,7 +109,7 @@ fabricUseGPUInterop = true [settings.exts."omni.kit.registry.nucleus"] registries = [ - { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/shared" }, + { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/107/shared" }, { name = "kit/sdk", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/sdk/${kit_version_short}/${kit_git_hash}" }, { name = "kit/community", url = "https://dw290v42wisod.cloudfront.net/exts/kit/community" }, ] @@ -145,6 +145,6 @@ folders = [ # set the S3 directory manually to the latest published S3 # note: this is done to ensure prior versions of Isaac Sim still use the latest assets [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaaclab.python.xr.openxr.headless.kit b/apps/isaaclab.python.xr.openxr.headless.kit index f9b89dc1b29..4fa2bfc0985 100644 --- a/apps/isaaclab.python.xr.openxr.headless.kit +++ b/apps/isaaclab.python.xr.openxr.headless.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python OpenXR Headless" description = "An app for running Isaac Lab with OpenXR in headless mode" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd", "headless"] @@ -15,16 +15,16 @@ keywords = ["experience", "app", "usd", "headless"] app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "5.0.0" +app.version = "5.1.0" ### FSD app.useFabricSceneDelegate = true # Temporary, should be enabled by default in Kit soon rtx.hydra.readTransformsFromFabricInRenderDelegate = true -# work around for kitxr issue -app.hydra.renderSettings.useUsdAttributes = false -app.hydra.renderSettings.useFabricAttributes = false +# xr optimizations +xr.skipInputDeviceUSDWrites = true +'rtx-transient'.resourcemanager.enableTextureStreaming = false [settings.isaaclab] # This is used to check that this experience file is loaded when using cameras @@ -59,6 +59,6 @@ folders = [ ] [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaaclab.python.xr.openxr.kit b/apps/isaaclab.python.xr.openxr.kit index c88fbe8ddc8..4150eae6449 100644 --- a/apps/isaaclab.python.xr.openxr.kit +++ b/apps/isaaclab.python.xr.openxr.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python OpenXR" description = "An app for running Isaac Lab with OpenXR" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] @@ -15,7 +15,7 @@ keywords = ["experience", "app", "usd"] app.versionFile = "${exe-path}/VERSION" app.folder = "${exe-path}/" app.name = "Isaac-Sim" -app.version = "5.0.0" +app.version = "5.1.0" ### async rendering settings # omni.replicator.asyncRendering needs to be false for external camera rendering @@ -32,9 +32,9 @@ app.useFabricSceneDelegate = true # Temporary, should be enabled by default in Kit soon rtx.hydra.readTransformsFromFabricInRenderDelegate = true -# work around for kitxr issue -app.hydra.renderSettings.useUsdAttributes = false -app.hydra.renderSettings.useFabricAttributes = false +# xr optimizations +xr.skipInputDeviceUSDWrites = true +'rtx-transient'.resourcemanager.enableTextureStreaming = false [dependencies] "isaaclab.python" = {} @@ -88,6 +88,6 @@ folders = [ # set the S3 directory manually to the latest published S3 # note: this is done to ensure prior versions of Isaac Sim still use the latest assets [settings] -persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" -persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0" +persistent.isaac.asset_root.default = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.cloud = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" +persistent.isaac.asset_root.nvidia = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1" diff --git a/apps/isaacsim_4_5/isaaclab.python.headless.kit b/apps/isaacsim_4_5/isaaclab.python.headless.kit index 13327588e0d..944e284c452 100644 --- a/apps/isaacsim_4_5/isaaclab.python.headless.kit +++ b/apps/isaacsim_4_5/isaaclab.python.headless.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python Headless" description = "An app for running Isaac Lab headlessly" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "headless"] diff --git a/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit b/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit index df06ee11a0b..cb1b4e8a25d 100644 --- a/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit +++ b/apps/isaacsim_4_5/isaaclab.python.headless.rendering.kit @@ -9,7 +9,7 @@ [package] title = "Isaac Lab Python Headless Camera" description = "An app for running Isaac Lab headlessly with rendering enabled" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] diff --git a/apps/isaacsim_4_5/isaaclab.python.kit b/apps/isaacsim_4_5/isaaclab.python.kit index 4b7f4086b66..89db9ffb0d6 100644 --- a/apps/isaacsim_4_5/isaaclab.python.kit +++ b/apps/isaacsim_4_5/isaaclab.python.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python" description = "An app for running Isaac Lab" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] diff --git a/apps/isaacsim_4_5/isaaclab.python.rendering.kit b/apps/isaacsim_4_5/isaaclab.python.rendering.kit index 8c319a040cd..df2ee90bf16 100644 --- a/apps/isaacsim_4_5/isaaclab.python.rendering.kit +++ b/apps/isaacsim_4_5/isaaclab.python.rendering.kit @@ -9,7 +9,7 @@ [package] title = "Isaac Lab Python Camera" description = "An app for running Isaac Lab with rendering enabled" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] diff --git a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit index f8b07af3383..5839ae8acc3 100644 --- a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit +++ b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.headless.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python OpenXR Headless" description = "An app for running Isaac Lab with OpenXR in headless mode" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd", "headless"] diff --git a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit index 663b7dfb4f3..24f4663c2e0 100644 --- a/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit +++ b/apps/isaacsim_4_5/isaaclab.python.xr.openxr.kit @@ -5,7 +5,7 @@ [package] title = "Isaac Lab Python OpenXR" description = "An app for running Isaac Lab with OpenXR" -version = "2.2.1" +version = "2.3.0" # That makes it browsable in UI with "experience" filter keywords = ["experience", "app", "usd"] diff --git a/docker/.env.base b/docker/.env.base index 5d34649b591..be1dd4f6221 100644 --- a/docker/.env.base +++ b/docker/.env.base @@ -6,8 +6,8 @@ ACCEPT_EULA=Y # NVIDIA Isaac Sim base image ISAACSIM_BASE_IMAGE=nvcr.io/nvidia/isaac-sim -# NVIDIA Isaac Sim version to use (e.g. 5.0.0) -ISAACSIM_VERSION=5.0.0 +# NVIDIA Isaac Sim version to use (e.g. 5.1.0) +ISAACSIM_VERSION=5.1.0 # Derived from the default path in the NVIDIA provided Isaac Sim container DOCKER_ISAACSIM_ROOT_PATH=/isaac-sim # The Isaac Lab path in the container diff --git a/docker/.env.cloudxr-runtime b/docker/.env.cloudxr-runtime index 3146b7a4f35..65b6d1373ac 100644 --- a/docker/.env.cloudxr-runtime +++ b/docker/.env.cloudxr-runtime @@ -5,4 +5,4 @@ # NVIDIA CloudXR Runtime base image CLOUDXR_RUNTIME_BASE_IMAGE_ARG=nvcr.io/nvidia/cloudxr-runtime # NVIDIA CloudXR Runtime version to use -CLOUDXR_RUNTIME_VERSION_ARG=5.0.0 +CLOUDXR_RUNTIME_VERSION_ARG=5.0.1 diff --git a/docs/Makefile b/docs/Makefile index ce33dad5033..0bff236671c 100644 --- a/docs/Makefile +++ b/docs/Makefile @@ -15,4 +15,5 @@ multi-docs: .PHONY: current-docs current-docs: - @$(SPHINXBUILD) "$(SOURCEDIR)" "$(BUILDDIR)/current" $(SPHINXOPTS) + @rm -rf "$(BUILDDIR)/current" + @$(SPHINXBUILD) -W --keep-going "$(SOURCEDIR)" "$(BUILDDIR)/current" $(SPHINXOPTS) diff --git a/docs/conf.py b/docs/conf.py index 3bdf99666ed..17cfb8000bf 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -96,6 +96,8 @@ (r"py:.*", r"trimesh.*"), # we don't have intersphinx mapping for trimesh ] +# emoji style +sphinxemoji_style = "twemoji" # options: "twemoji" or "unicode" # put type hints inside the signature instead of the description (easier to maintain) autodoc_typehints = "signature" # autodoc_typehints_format = "fully-qualified" @@ -123,7 +125,7 @@ "numpy": ("https://numpy.org/doc/stable/", None), "trimesh": ("https://trimesh.org/", None), "torch": ("https://pytorch.org/docs/stable/", None), - "isaacsim": ("https://docs.isaacsim.omniverse.nvidia.com/5.0.0/py/", None), + "isaacsim": ("https://docs.isaacsim.omniverse.nvidia.com/5.1.0/py/", None), "gymnasium": ("https://gymnasium.farama.org/", None), "warp": ("https://nvidia.github.io/warp/", None), "dev-guide": ("https://docs.omniverse.nvidia.com/dev-guide/latest", None), @@ -259,7 +261,7 @@ { "name": "Isaac Sim", "url": "https://developer.nvidia.com/isaac-sim", - "icon": "https://img.shields.io/badge/IsaacSim-5.0.0-silver.svg", + "icon": "https://img.shields.io/badge/IsaacSim-5.1.0-silver.svg", "type": "url", }, { @@ -279,7 +281,7 @@ # Whitelist pattern for remotes smv_remote_whitelist = r"^.*$" # Whitelist pattern for branches (set to None to ignore all branches) -smv_branch_whitelist = os.getenv("SMV_BRANCH_WHITELIST", r"^(main|devel)$") +smv_branch_whitelist = os.getenv("SMV_BRANCH_WHITELIST", r"^(main|devel|release/.*)$") # Whitelist pattern for tags (set to None to ignore all tags) smv_tag_whitelist = os.getenv("SMV_TAG_WHITELIST", r"^v[1-9]\d*\.\d+\.\d+$") html_sidebars = { diff --git a/docs/index.rst b/docs/index.rst index baeeffdd35f..fbffccd6820 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -74,12 +74,13 @@ Table of Contents ================= .. toctree:: - :maxdepth: 2 + :maxdepth: 1 :caption: Isaac Lab source/setup/ecosystem source/setup/installation/index source/deployment/index + source/setup/installation/cloud_installation source/refs/reference_architecture/index @@ -116,6 +117,7 @@ Table of Contents source/features/hydra source/features/multi_gpu + source/features/population_based_training Tiled Rendering source/features/ray source/features/reproducibility diff --git a/docs/licenses/dependencies/jsonschema-license b/docs/licenses/dependencies/jsonschema-license.txt similarity index 100% rename from docs/licenses/dependencies/jsonschema-license rename to docs/licenses/dependencies/jsonschema-license.txt diff --git a/docs/licenses/dependencies/jsonschema-specifications-license b/docs/licenses/dependencies/jsonschema-specifications-license.txt similarity index 100% rename from docs/licenses/dependencies/jsonschema-specifications-license rename to docs/licenses/dependencies/jsonschema-specifications-license.txt diff --git a/docs/licenses/dependencies/labeler-license.txt b/docs/licenses/dependencies/labeler-license.txt new file mode 100644 index 00000000000..cfbc8bb6dda --- /dev/null +++ b/docs/licenses/dependencies/labeler-license.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2018 GitHub, Inc. and contributors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/docs/licenses/dependencies/pinocchio-license.txt b/docs/licenses/dependencies/pinocchio-license.txt new file mode 100644 index 00000000000..dfacb673148 --- /dev/null +++ b/docs/licenses/dependencies/pinocchio-license.txt @@ -0,0 +1,26 @@ +BSD 2-Clause License + +Copyright (c) 2014-2023, CNRS +Copyright (c) 2018-2025, INRIA +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/docs/licenses/dependencies/pygame-license b/docs/licenses/dependencies/pygame-license.txt similarity index 100% rename from docs/licenses/dependencies/pygame-license rename to docs/licenses/dependencies/pygame-license.txt diff --git a/docs/licenses/dependencies/referencing-license b/docs/licenses/dependencies/referencing-license.txt similarity index 100% rename from docs/licenses/dependencies/referencing-license rename to docs/licenses/dependencies/referencing-license.txt diff --git a/docs/licenses/dependencies/typing-inspection-license b/docs/licenses/dependencies/typing-inspection-license.txt similarity index 100% rename from docs/licenses/dependencies/typing-inspection-license rename to docs/licenses/dependencies/typing-inspection-license.txt diff --git a/docs/licenses/dependencies/zipp-license b/docs/licenses/dependencies/zipp-license.txt similarity index 100% rename from docs/licenses/dependencies/zipp-license rename to docs/licenses/dependencies/zipp-license.txt diff --git a/docs/make.bat b/docs/make.bat index cdaf22f257c..676a3abc67d 100644 --- a/docs/make.bat +++ b/docs/make.bat @@ -1,64 +1,65 @@ -@ECHO OFF - -pushd %~dp0 - -REM Command file to build Sphinx documentation - -set SOURCEDIR=. -set BUILDDIR=_build - -REM Check if a specific target was passed -if "%1" == "multi-docs" ( - REM Check if SPHINXBUILD is set, if not default to sphinx-multiversion - if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-multiversion - ) - %SPHINXBUILD% >NUL 2>NUL - if errorlevel 9009 ( - echo. - echo.The 'sphinx-multiversion' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-multiversion' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 - ) - %SPHINXBUILD% %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% - - REM Copy the redirect index.html to the build directory - copy _redirect\index.html %BUILDDIR%\index.html - goto end -) - -if "%1" == "current-docs" ( - REM Check if SPHINXBUILD is set, if not default to sphinx-build - if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-build - ) - %SPHINXBUILD% >NUL 2>NUL - if errorlevel 9009 ( - echo. - echo.The 'sphinx-build' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-build' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 - ) - %SPHINXBUILD% %SOURCEDIR% %BUILDDIR%\current %SPHINXOPTS% %O% - goto end -) - -REM If no valid target is passed, show usage instructions -echo. -echo.Usage: -echo. make.bat multi-docs - To build the multi-version documentation. -echo. make.bat current-docs - To build the current documentation. -echo. - -:end -popd +@ECHO OFF + +pushd %~dp0 + +REM Command file to build Sphinx documentation + +set SOURCEDIR=. +set BUILDDIR=_build + +REM Check if a specific target was passed +if "%1" == "multi-docs" ( + REM Check if SPHINXBUILD is set, if not default to sphinx-multiversion + if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-multiversion + ) + where %SPHINXBUILD% >NUL 2>NUL + if errorlevel 1 ( + echo. + echo.The 'sphinx-multiversion' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-multiversion' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 + ) + %SPHINXBUILD% %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + + REM Copy the redirect index.html to the build directory + copy _redirect\index.html %BUILDDIR%\index.html + goto end +) + +if "%1" == "current-docs" ( + REM Check if SPHINXBUILD is set, if not default to sphinx-build + if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build + ) + where %SPHINXBUILD% >NUL 2>NUL + if errorlevel 1 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 + ) + if exist "%BUILDDIR%\current" rmdir /s /q "%BUILDDIR%\current" + %SPHINXBUILD% -W "%SOURCEDIR%" "%BUILDDIR%\current" %SPHINXOPTS% + goto end +) + +REM If no valid target is passed, show usage instructions +echo. +echo.Usage: +echo. make.bat multi-docs - To build the multi-version documentation. +echo. make.bat current-docs - To build the current documentation. +echo. + +:end +popd diff --git a/docs/source/_static/tasks/manipulation/agibot_place_mug.jpg b/docs/source/_static/tasks/manipulation/agibot_place_mug.jpg new file mode 100644 index 00000000000..73a421714c2 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/agibot_place_mug.jpg differ diff --git a/docs/source/_static/tasks/manipulation/agibot_place_toy.jpg b/docs/source/_static/tasks/manipulation/agibot_place_toy.jpg new file mode 100644 index 00000000000..15f41ba7fb4 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/agibot_place_toy.jpg differ diff --git a/docs/source/_static/tasks/manipulation/g1_pick_place.jpg b/docs/source/_static/tasks/manipulation/g1_pick_place.jpg new file mode 100644 index 00000000000..86d2180c058 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/g1_pick_place.jpg differ diff --git a/docs/source/_static/tasks/manipulation/g1_pick_place_fixed_base.jpg b/docs/source/_static/tasks/manipulation/g1_pick_place_fixed_base.jpg new file mode 100644 index 00000000000..ce9d73c8dcd Binary files /dev/null and b/docs/source/_static/tasks/manipulation/g1_pick_place_fixed_base.jpg differ diff --git a/docs/source/_static/tasks/manipulation/g1_pick_place_locomanipulation.jpg b/docs/source/_static/tasks/manipulation/g1_pick_place_locomanipulation.jpg new file mode 100644 index 00000000000..45d712c2c74 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/g1_pick_place_locomanipulation.jpg differ diff --git a/docs/source/_static/tasks/manipulation/kuka_allegro_lift.jpg b/docs/source/_static/tasks/manipulation/kuka_allegro_lift.jpg new file mode 100644 index 00000000000..9d19b0e4236 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/kuka_allegro_lift.jpg differ diff --git a/docs/source/_static/tasks/manipulation/kuka_allegro_reorient.jpg b/docs/source/_static/tasks/manipulation/kuka_allegro_reorient.jpg new file mode 100644 index 00000000000..384e7632e44 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/kuka_allegro_reorient.jpg differ diff --git a/docs/source/_static/teleop/hand_asset.jpg b/docs/source/_static/teleop/hand_asset.jpg new file mode 100755 index 00000000000..240b784673b Binary files /dev/null and b/docs/source/_static/teleop/hand_asset.jpg differ diff --git a/docs/source/_static/teleop/teleop_diagram.jpg b/docs/source/_static/teleop/teleop_diagram.jpg new file mode 100755 index 00000000000..48d6c29e7d7 Binary files /dev/null and b/docs/source/_static/teleop/teleop_diagram.jpg differ diff --git a/docs/source/api/lab/isaaclab.assets.rst b/docs/source/api/lab/isaaclab.assets.rst index 338d729ddb6..c91066966e8 100644 --- a/docs/source/api/lab/isaaclab.assets.rst +++ b/docs/source/api/lab/isaaclab.assets.rst @@ -32,7 +32,7 @@ Asset Base .. autoclass:: AssetBaseCfg :members: - :exclude-members: __init__, class_type + :exclude-members: __init__, class_type, InitialStateCfg Rigid Object ------------ diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index 17ce71e3827..c30ed948f09 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -61,7 +61,7 @@ USD Camera :members: :inherited-members: :show-inheritance: - :exclude-members: __init__, class_type + :exclude-members: __init__, class_type, OffsetCfg Tile-Rendered USD Camera ------------------------ diff --git a/docs/source/deployment/cloudxr_teleoperation_cluster.rst b/docs/source/deployment/cloudxr_teleoperation_cluster.rst index f027e9d8a0c..9548e29eb70 100644 --- a/docs/source/deployment/cloudxr_teleoperation_cluster.rst +++ b/docs/source/deployment/cloudxr_teleoperation_cluster.rst @@ -15,6 +15,9 @@ System Requirements * **Minimum requirement**: Kubernetes cluster with a node that has at least 1 NVIDIA RTX PRO 6000 / L40 GPU or equivalent * **Recommended requirement**: Kubernetes cluster with a node that has at least 2 RTX PRO 6000 / L40 GPUs or equivalent +.. note:: + If you are using DGX Spark, check `DGX Spark Limitations `_ for compatibility. + Software Dependencies --------------------- @@ -76,7 +79,7 @@ Installation .. code:: bash - helm fetch https://helm.ngc.nvidia.com/nvidia/charts/isaac-lab-teleop-2.2.0.tgz \ + helm fetch https://helm.ngc.nvidia.com/nvidia/charts/isaac-lab-teleop-2.3.0.tgz \ --username='$oauthtoken' \ --password= @@ -84,7 +87,7 @@ Installation .. code:: bash - helm upgrade --install hello-isaac-teleop isaac-lab-teleop-2.2.0.tgz \ + helm upgrade --install hello-isaac-teleop isaac-lab-teleop-2.3.0.tgz \ --set fullnameOverride=hello-isaac-teleop \ --set hostNetwork="true" @@ -107,7 +110,7 @@ Installation # command helm upgrade --install --values local_values.yml \ - hello-isaac-teleop isaac-lab-teleop-2.2.0.tgz + hello-isaac-teleop isaac-lab-teleop-2.3.0.tgz #. Verify the deployment is completed: diff --git a/docs/source/deployment/docker.rst b/docs/source/deployment/docker.rst index 6d8e648da52..71849189326 100644 --- a/docs/source/deployment/docker.rst +++ b/docs/source/deployment/docker.rst @@ -307,7 +307,7 @@ To pull the minimal Isaac Lab container, run: .. code:: bash - docker pull nvcr.io/nvidia/isaac-lab:2.2.0 + docker pull nvcr.io/nvidia/isaac-lab:2.3.0 To run the Isaac Lab container with an interactive bash session, run: @@ -323,7 +323,7 @@ To run the Isaac Lab container with an interactive bash session, run: -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ -v ~/docker/isaac-sim/documents:/root/Documents:rw \ - nvcr.io/nvidia/isaac-lab:2.2.0 + nvcr.io/nvidia/isaac-lab:2.3.0 To enable rendering through X11 forwarding, run: @@ -342,7 +342,7 @@ To enable rendering through X11 forwarding, run: -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ -v ~/docker/isaac-sim/documents:/root/Documents:rw \ - nvcr.io/nvidia/isaac-lab:2.2.0 + nvcr.io/nvidia/isaac-lab:2.3.0 To run an example within the container, run: diff --git a/docs/source/deployment/index.rst b/docs/source/deployment/index.rst index a7791a395e6..235a23c9d75 100644 --- a/docs/source/deployment/index.rst +++ b/docs/source/deployment/index.rst @@ -1,3 +1,5 @@ +.. _container-deployment: + Container Deployment ==================== @@ -11,13 +13,65 @@ The Dockerfile is based on the Isaac Sim image provided by NVIDIA, which include application launcher and the Isaac Sim application. The Dockerfile installs Isaac Lab and its dependencies on top of this image. -The following guides provide instructions for building the Docker image and running Isaac Lab in a -container. +Cloning the Repository +---------------------- + +Before building the container, clone the Isaac Lab repository (if not already done): + +.. tab-set:: + + .. tab-item:: SSH + + .. code:: bash + + git clone git@github.com:isaac-sim/IsaacLab.git + + .. tab-item:: HTTPS + + .. code:: bash + + git clone https://github.com/isaac-sim/IsaacLab.git + +Next Steps +---------- + +After cloning, you can choose the deployment workflow that fits your needs: + +- :doc:`docker` + + - Learn how to build, configure, and run Isaac Lab in Docker containers. + - Explains the repository's ``docker/`` setup, the ``container.py`` helper script, mounted volumes, + image extensions (like ROS 2), and optional CloudXR streaming support. + - Covers running pre-built Isaac Lab containers from NVIDIA NGC for headless training. + +- :doc:`run_docker_example` + + - Learn how to run a development workflow inside the Isaac Lab Docker container. + - Demonstrates building the container, entering it, executing a sample Python script (`log_time.py`), + and retrieving logs using mounted volumes. + - Highlights bind-mounted directories for live code editing and explains how to stop or remove the container + while keeping the image and artifacts. + +- :doc:`cluster` + + - Learn how to run Isaac Lab on high-performance computing (HPC) clusters. + - Explains how to export the Docker image to a Singularity (Apptainer) image, configure cluster-specific parameters, + and submit jobs using common workload managers (SLURM or PBS). + - Includes tested workflows for ETH Zurich's Euler cluster and IIT Genoa's Franklin cluster, + with notes on adapting to other environments. + +- :doc:`cloudxr_teleoperation_cluster` + + - Deploy CloudXR Teleoperation for Isaac Lab on a Kubernetes cluster. + - Covers system requirements, software dependencies, and preparation steps including RBAC permissions. + - Demonstrates how to install and verify the Helm chart, run the pod, and uninstall it. + .. toctree:: - :maxdepth: 1 + :maxdepth: 1 + :hidden: - docker - cluster - cloudxr_teleoperation_cluster - run_docker_example + docker + run_docker_example + cluster + cloudxr_teleoperation_cluster diff --git a/docs/source/experimental-features/newton-physics-integration/installation.rst b/docs/source/experimental-features/newton-physics-integration/installation.rst index 158aeca495b..fc59e188b52 100644 --- a/docs/source/experimental-features/newton-physics-integration/installation.rst +++ b/docs/source/experimental-features/newton-physics-integration/installation.rst @@ -44,7 +44,7 @@ Install the correct version of torch and torchvision: .. code-block:: bash - pip install torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 Install Isaac Sim 5.0: diff --git a/docs/source/experimental-features/newton-physics-integration/limitations-and-known-bugs.rst b/docs/source/experimental-features/newton-physics-integration/limitations-and-known-bugs.rst index d656f0ea406..b7499042540 100644 --- a/docs/source/experimental-features/newton-physics-integration/limitations-and-known-bugs.rst +++ b/docs/source/experimental-features/newton-physics-integration/limitations-and-known-bugs.rst @@ -9,28 +9,48 @@ We do not expect to be able to provide support or debugging assistance until the Here is a non-exhaustive list of capabilities currently supported in the Newton experimental feature branch grouped by extension: * isaaclab: - * Articulation API + * Articulation API (supports both articulations and single-body articulations as rigid bodies) * Contact Sensor * Direct & Manager single agent workflows * Omniverse Kit visualizer * Newton visualizer * isaaclab_assets: - * Anymal-D - * Unitree H1 & G1 + * Quadrupeds + * Anymal-B, Anymal-C, Anymal-D + * Unitree A1, Go1, Go2 + * Spot + * Humanoids + * Unitree H1 & G1 + * Cassie + * Arms and Hands + * Franka + * UR10 + * Allegro Hand * Toy examples * Cartpole * Ant * Humanoid * isaaclab_tasks: * Direct: - * Cartpole + * Cartpole (State, RGB, Depth) * Ant * Humanoid + * Allegro Hand Repose Cube * Manager based: + * Cartpole (State) + * Ant + * Humanoid * Locomotion (velocity flat terrain) + * Anymal-B + * Anymal-C * Anymal-D + * Cassie + * A1 + * Go1 + * Go2 + * Spot * Unitree G1 * Unitree H1 - -Capabilities beyond the above are not currently available. -We expect to support APIs related to rigid bodies soon in order to unlock manipulation based environments. + * Manipulation reach + * Franka + * UR10 diff --git a/docs/source/experimental-features/newton-physics-integration/newton-visualizer.rst b/docs/source/experimental-features/newton-physics-integration/newton-visualizer.rst index d59001d1810..9efc7639bfb 100644 --- a/docs/source/experimental-features/newton-physics-integration/newton-visualizer.rst +++ b/docs/source/experimental-features/newton-physics-integration/newton-visualizer.rst @@ -32,4 +32,8 @@ lower number of environments, we can omit the ``--headless`` option while still These options are available across all the learning frameworks. -For more information about the Newton Visualizer, please refer to the `Newton documentation `_ . +For more information about the Newton Visualizer, please refer to the `Newton documentation `_. + +IsaacLab provides additional customizations to the Newton visualizer with several learning-oriented features. These include the ability to pause rendering during training or pause the training process itself. Pausing rendering accelerates training by skipping rendering frames, which is particularly useful when we want to periodically check the trained policy without the performance overhead of continuous rendering. Pausing the training process is valuable for debugging purposes. Additionally, the visualizer's update frequency can be adjusted using a slider in the visualizer window, making it easy to prioritize rendering quality against training performance and vice-versa. + +All IsaacLab-specific customizations are organized under the *IsaacLab Training Controls* tab in the visualizer window. diff --git a/docs/source/experimental-features/newton-physics-integration/sim-to-sim.rst b/docs/source/experimental-features/newton-physics-integration/sim-to-sim.rst index 24f791aa9cb..3ccc8807cc6 100644 --- a/docs/source/experimental-features/newton-physics-integration/sim-to-sim.rst +++ b/docs/source/experimental-features/newton-physics-integration/sim-to-sim.rst @@ -2,38 +2,40 @@ Sim-to-Sim Policy Transfer ========================== -This section provides examples of sim-to-sim policy transfer using the Newton backend. Sim-to-sim transfer is an essential step before real robot deployment because it verifies that policies work across different simulators. Policies that pass sim-to-sim verification are much more likely to succeed on real robots. +This section provides examples of sim-to-sim policy transfer between PhysX and Newton backends. Sim-to-sim transfer is an essential step before real robot deployment because it verifies that policies work across different simulators. Policies that pass sim-to-sim verification are much more likely to succeed on real robots. Overview -------- -This guide shows how to run a PhysX-trained policy on the Newton backend. While the method works for any robot and physics engine, it has only been tested with Unitree G1, Unitree H1, and ANYmal-D robots using PhysX-trained policies. +This guide shows how to transfer policies between PhysX and Newton backends in both directions. The main challenge is that different physics engines may parse the same robot model with different joint and link ordering. -PhysX-trained policies expect joints and links in a specific order determined by how PhysX parses the robot model. However, Newton may parse the same robot with different joint and link ordering. +Policies trained in one backend expect joints and links in a specific order determined by how that backend parses the robot model. When transferring to another backend, the joint ordering may be different, requiring remapping of observations and actions. In the future, we plan to solve this using **robot schema** that standardizes joint and link ordering across different backends. -Currently, we solve this by remapping observations and actions using joint mappings defined in YAML files. These files specify joint names in both PhysX order (source) and Newton order (target). During policy execution, we use this mapping to reorder observations and actions so they work correctly with Newton. +Currently, we solve this by remapping observations and actions using joint mappings defined in YAML files. These files specify joint names in both source and target backend orders. During policy execution, we use this mapping to reorder observations and actions so they work correctly with the target backend. + +The method has been tested with Unitree G1, Unitree Go2, Unitree H1, and ANYmal-D robots for both transfer directions. What you need ~~~~~~~~~~~~~ -- A policy checkpoint trained with PhysX (RSL-RL). -- A joint mapping YAML for your robot under ``scripts/newton_sim2sim/mappings/``. -- The provided player script: ``scripts/newton_sim2sim/rsl_rl_transfer.py``. +- A policy checkpoint trained with either PhysX or Newton (RSL-RL). +- A joint mapping YAML for your robot under ``scripts/sim2sim_transfer/config/``. +- The provided player script: ``scripts/sim2sim_transfer/rsl_rl_transfer.py``. To add a new robot, create a YAML file with two lists where each joint name appears exactly once in both: .. code-block:: yaml # Example structure - source_joint_names: # PhysX joint order + source_joint_names: # Source backend joint order - joint_1 - joint_2 # ... - target_joint_names: # Newton joint order + target_joint_names: # Target backend joint order - joint_1 - joint_2 # ... @@ -41,14 +43,14 @@ To add a new robot, create a YAML file with two lists where each joint name appe The script automatically computes the necessary mappings for locomotion tasks. -How to run -~~~~~~~~~~ +PhysX-to-Newton Transfer +~~~~~~~~~~~~~~~~~~~~~~~~ -Use this command template to run a PhysX-trained policy with Newton: +To run a PhysX-trained policy with the Newton backend, use this command template: .. code-block:: bash - ./isaaclab.sh -p scripts/newton_sim2sim/rsl_rl_transfer.py \ + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ --task= \ --num_envs=32 \ --checkpoint \ @@ -60,11 +62,11 @@ Here are examples for different robots: .. code-block:: bash - ./isaaclab.sh -p scripts/newton_sim2sim/rsl_rl_transfer.py \ + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ --task=Isaac-Velocity-Flat-G1-v0 \ --num_envs=32 \ --checkpoint \ - --policy_transfer_file scripts/newton_sim2sim/mappings/sim2sim_g1.yaml + --policy_transfer_file scripts/sim2sim_transfer/config/physx_to_newton_g1.yaml 2. Unitree H1 @@ -72,28 +74,94 @@ Here are examples for different robots: .. code-block:: bash - ./isaaclab.sh -p scripts/newton_sim2sim/rsl_rl_transfer.py \ + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ --task=Isaac-Velocity-Flat-H1-v0 \ --num_envs=32 \ --checkpoint \ - --policy_transfer_file scripts/newton_sim2sim/mappings/sim2sim_h1.yaml + --policy_transfer_file scripts/sim2sim_transfer/config/physx_to_newton_h1.yaml + + +3. Unitree Go2 +.. code-block:: bash -3. ANYmal-D + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task=Isaac-Velocity-Flat-Go2-v0 \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file scripts/sim2sim_transfer/config/physx_to_newton_go2.yaml + + +4. ANYmal-D .. code-block:: bash - ./isaaclab.sh -p scripts/newton_sim2sim/rsl_rl_transfer.py \ + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ --task=Isaac-Velocity-Flat-Anymal-D-v0 \ --num_envs=32 \ --checkpoint \ - --policy_transfer_file scripts/newton_sim2sim/mappings/sim2sim_anymal_d.yaml + --policy_transfer_file scripts/sim2sim_transfer/config/physx_to_newton_anymal_d.yaml + +Note that to run this, you need to checkout the Newton-based branch of IsaacLab such as ``feature/newton``. + +Newton-to-PhysX Transfer +~~~~~~~~~~~~~~~~~~~~~~~~ + +To transfer Newton-trained policies to PhysX-based IsaacLab, use the reverse mapping files: + +Here are examples for different robots: + +1. Unitree G1 + +.. code-block:: bash + + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task=Isaac-Velocity-Flat-G1-v0 \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file scripts/sim2sim_transfer/config/newton_to_physx_g1.yaml + + +2. Unitree H1 + +.. code-block:: bash + + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task=Isaac-Velocity-Flat-H1-v0 \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file scripts/sim2sim_transfer/config/newton_to_physx_h1.yaml + + +3. Unitree Go2 + +.. code-block:: bash + + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task=Isaac-Velocity-Flat-Go2-v0 \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file scripts/sim2sim_transfer/config/newton_to_physx_go2.yaml + + +4. ANYmal-D + +.. code-block:: bash + + ./isaaclab.sh -p scripts/sim2sim_transfer/rsl_rl_transfer.py \ + --task=Isaac-Velocity-Flat-Anymal-D-v0 \ + --num_envs=32 \ + --checkpoint \ + --policy_transfer_file scripts/sim2sim_transfer/config/newton_to_physx_anymal_d.yaml +The key difference is using the ``newton_to_physx_*.yaml`` mapping files instead of ``physx_to_newton_*.yaml`` files. Also note that you need to checkout a PhysX-based IsaacLab branch such as ``main``. -Notes and limitations +Notes and Limitations ~~~~~~~~~~~~~~~~~~~~~ -- This transfer method has only been tested with Unitree G1, Unitree H1, and ANYmal-D using PhysX-trained policies. -- The observation remapping assumes a locomotion layout with base observations followed by joint observations. For different observation layouts, you'll need to modify ``scripts/newton_sim2sim/policy_mapping.py``. +- Both transfer directions have been tested with Unitree G1, Unitree Go2, Unitree H1, and ANYmal-D robots. +- PhysX-to-Newton transfer uses ``physx_to_newton_*.yaml`` mapping files. +- Newton-to-PhysX transfer requires the corresponding ``newton_to_physx_*.yaml`` mapping files and the PhysX branch of IsaacLab. +- The observation remapping assumes a locomotion layout with base observations followed by joint observations. For different observation layouts, you'll need to modify the ``get_joint_mappings`` function in ``scripts/sim2sim_transfer/rsl_rl_transfer.py``. - When adding new robots or backends, make sure both source and target have identical joint names, and that the YAML lists reflect how each backend orders these joints. diff --git a/docs/source/experimental-features/newton-physics-integration/training-environments.rst b/docs/source/experimental-features/newton-physics-integration/training-environments.rst index 7ef5619661e..ef98339e5e6 100644 --- a/docs/source/experimental-features/newton-physics-integration/training-environments.rst +++ b/docs/source/experimental-features/newton-physics-integration/training-environments.rst @@ -6,12 +6,28 @@ To run training, we follow the standard Isaac Lab workflow. If you are new to Is The currently supported tasks are as follows: * Isaac-Cartpole-Direct-v0 +* Isaac-Cartpole-RGB-Camera-Direct-v0 (requires ``--enable_cameras``) +* Isaac-Cartpole-Depth-Camera-Direct-v0 (requires ``--enable_cameras``) +* Isaac-Cartpole-v0 * Isaac-Ant-Direct-v0 +* Isaac-Ant-v0 * Isaac-Humanoid-Direct-v0 +* Isaac-Humanoid-v0 +* Isaac-Velocity-Flat-Anymal-B-v0 +* Isaac-Velocity-Flat-Anymal-C-v0 * Isaac-Velocity-Flat-Anymal-D-v0 +* Isaac-Velocity-Flat-Cassie-v0 * Isaac-Velocity-Flat-G1-v0 * Isaac-Velocity-Flat-G1-v1 (Sim-to-Real tested) * Isaac-Velocity-Flat-H1-v0 +* Isaac-Velocity-Flat-Unitree-A1-v0 +* Isaac-Velocity-Flat-Unitree-Go1-v0 +* Isaac-Velocity-Flat-Unitree-Go2-v0 +* Isaac-Velocity-Flat-Spot-v0 +* Isaac-Reach-Franka-v0 +* Isaac-Reach-UR10-v0 +* Isaac-Repose-Cube-Allegro-Direct-v0 + To launch an environment and check that it loads as expected, we can start by trying it out with zero actions sent to its actuators. This can be done as follows, where ``TASK_NAME`` is the name of the task you’d like to run, and ``NUM_ENVS`` is the number of instances of the task that you’d like to create. diff --git a/docs/source/features/population_based_training.rst b/docs/source/features/population_based_training.rst new file mode 100644 index 00000000000..d88b8195bc7 --- /dev/null +++ b/docs/source/features/population_based_training.rst @@ -0,0 +1,140 @@ +Population Based Training +========================= + +What PBT Does +------------- + +* Trains *N* policies in parallel (a "population") on the **same task**. +* Every ``interval_steps``: + + #. Save each policy's checkpoint and objective. + #. Score the population and identify **leaders** and **underperformers**. + #. For underperformers, replace weights from a random leader and **mutate** selected hyperparameters. + #. Restart that process with the new weights/params automatically. + +Leader / Underperformer Selection +--------------------------------- + +Let ``o_i`` be each initialized policy's objective, with mean ``μ`` and std ``σ``. + +Upper and lower performance cuts are:: + + upper_cut = max(μ + threshold_std * σ, μ + threshold_abs) + lower_cut = min(μ - threshold_std * σ, μ - threshold_abs) + +* **Leaders**: ``o_i > upper_cut`` +* **Underperformers**: ``o_i < lower_cut`` + +The "Natural-Selection" rules: + +1. Only underperformers are acted on (mutated or replaced). +2. If leaders exist, replace an underperformer with a random leader; otherwise, self-mutate. + +Mutation (Hyperparameters) +-------------------------- + +* Each param has a mutation function (e.g., ``mutate_float``, ``mutate_discount``, etc.). +* A param is mutated with probability ``mutation_rate``. +* When mutated, its value is perturbed within ``change_range = (min, max)``. +* Only whitelisted keys (from the PBT config) are considered. + +Example Config +-------------- + +.. code-block:: yaml + + pbt: + enabled: True + policy_idx: 0 + num_policies: 8 + directory: . + workspace: "pbt_workspace" + objective: episode.Curriculum/difficulty_level + interval_steps: 50000000 + threshold_std: 0.1 + threshold_abs: 0.025 + mutation_rate: 0.25 + change_range: [1.1, 2.0] + mutation: + agent.params.config.learning_rate: "mutate_float" + agent.params.config.grad_norm: "mutate_float" + agent.params.config.entropy_coef: "mutate_float" + agent.params.config.critic_coef: "mutate_float" + agent.params.config.bounds_loss_coef: "mutate_float" + agent.params.config.kl_threshold: "mutate_float" + agent.params.config.gamma: "mutate_discount" + agent.params.config.tau: "mutate_discount" + + +``objective: episode.Curriculum/difficulty_level`` is the dotted expression that uses +``infos["episode"]["Curriculum/difficulty_level"]`` as the scalar to **rank policies** (higher is better). +With ``num_policies: 8``, launch eight processes sharing the same ``workspace`` and unique ``policy_idx`` (0-7). + + +Launching PBT +------------- + +You must start **one process per policy** and point them to the **same workspace**. Set a unique +``policy_idx`` for each process and the common ``num_policies``. + +Minimal flags you need: + +* ``agent.pbt.enabled=True`` +* ``agent.pbt.directory=`` +* ``agent.pbt.policy_idx=<0..num_policies-1>`` + +.. note:: + All processes must use the same ``agent.pbt.workspace`` so they can see each other's checkpoints. + +.. caution:: + PBT is currently supported **only** with the **rl_games** library. Other RL libraries are not supported yet. + +Tips +---- + +* Keep checkpoints reasonable: reduce ``interval_steps`` only if you really need tighter PBT cadence. +* Use larger ``threshold_std`` and ``threshold_abs`` for greater population diversity. +* It is recommended to run 6+ workers to see benefit of pbt. + + +Training Example +---------------- + +We provide a reference PPO config here for task: +`Isaac-Dexsuite-Kuka-Allegro-Lift-v0 `_. +For the best logging experience, we recommend using wandb for the logging in the script. + +Launch *N* workers, where *n* indicates each worker index: + +.. code-block:: bash + + # Run this once per worker (n = 0..N-1), all pointing to the same directory/workspace + ./isaaclab.sh -p scripts/reinforcement_learning/rl_games/train.py \ + --seed= \ + --task=Isaac-Dexsuite-Kuka-Allegro-Lift-v0 \ + --num_envs=8192 \ + --headless \ + --track \ + --wandb-name=idx \ + --wandb-entity=<**entity**> \ + --wandb-project-name=<**project**> + agent.pbt.enabled=True \ + agent.pbt.num_policies= \ + agent.pbt.policy_idx= \ + agent.pbt.workspace=<**pbt_workspace_name**> \ + agent.pbt.directory=<**/path/to/shared_folder**> \ + + +References +---------- + +This PBT implementation reimplements and is inspired by *Dexpbt: Scaling up dexterous manipulation for hand-arm systems with population based training* (Petrenko et al., 2023). + +.. code-block:: bibtex + + @article{petrenko2023dexpbt, + title={Dexpbt: Scaling up dexterous manipulation for hand-arm systems with population based training}, + author={Petrenko, Aleksei and Allshire, Arthur and State, Gavriel and Handa, Ankur and Makoviychuk, Viktor}, + journal={arXiv preprint arXiv:2305.12127}, + year={2023} + } diff --git a/docs/source/features/ray.rst b/docs/source/features/ray.rst index 98367fac174..959fb518eb5 100644 --- a/docs/source/features/ray.rst +++ b/docs/source/features/ray.rst @@ -16,6 +16,13 @@ the general workflow is the same. This functionality is experimental, and has been tested only on Linux. +.. warning:: + + **Security Notice**: Due to security risks associated with Ray, + this workflow is not intended for use outside of a strictly controlled + network environment. Ray clusters should only be deployed in trusted, + isolated networks with appropriate access controls and security measures in place. + Overview diff --git a/docs/source/how-to/add_own_library.rst b/docs/source/how-to/add_own_library.rst index d0cb9dd5a62..8a0347d6597 100644 --- a/docs/source/how-to/add_own_library.rst +++ b/docs/source/how-to/add_own_library.rst @@ -1,3 +1,5 @@ +.. _how-to-add-library: + Adding your own learning library ================================ @@ -47,7 +49,7 @@ For instance, if you cloned the library to ``/home/user/git/rsl_rl``, the output .. code-block:: bash Name: rsl_rl - Version: 2.2.0 + Version: 3.0.1 Summary: Fast and simple RL algorithms implemented in pytorch Home-page: https://github.com/leggedrobotics/rsl_rl Author: ETH Zurich, NVIDIA CORPORATION diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index 3a03b283589..4c9a0671622 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -22,7 +22,7 @@ teleoperation in Isaac Lab. .. note:: - Support for additional devices is planned for future releases. + See :ref:`manus-vive-handtracking` for more information on supported hand-tracking peripherals. Overview @@ -43,8 +43,8 @@ This guide will walk you through how to: * :ref:`run-isaac-lab-with-the-cloudxr-runtime` -* :ref:`use-apple-vision-pro`, including how to :ref:`build-apple-vision-pro` and - :ref:`teleoperate-apple-vision-pro`. +* :ref:`use-apple-vision-pro`, including how to :ref:`build-apple-vision-pro`, + :ref:`teleoperate-apple-vision-pro`, and :ref:`manus-vive-handtracking`. * :ref:`develop-xr-isaac-lab`, including how to :ref:`run-isaac-lab-with-xr`, :ref:`configure-scene-placement`, and :ref:`optimize-xr-performance`. @@ -63,17 +63,17 @@ Prior to using CloudXR with Isaac Lab, please review the following system requir * Isaac Lab workstation * Ubuntu 22.04 or Ubuntu 24.04 + * Hardware requirements to sustain 45 FPS with a 120Hz physics simulation: + * CPU: 16-Cores AMD Ryzen Threadripper Pro 5955WX or higher + * Memory: 64GB RAM + * GPU: 1x RTX PRO 6000 GPUs (or equivalent e.g. 1x RTX 5090) or higher + * For details on driver requirements, please see the `Technical Requirements `_ guide * `Docker`_ 26.0.0+, `Docker Compose`_ 2.25.0+, and the `NVIDIA Container Toolkit`_. Refer to the Isaac Lab :ref:`deployment-docker` for how to install. - * For details on driver requirements, please see the `Technical Requirements `_ guide - * Required for best performance: 16 cores Intel Core i9, X-series or higher AMD Ryzen 9, - Threadripper or higher - * Required for best performance: 64GB RAM - * Required for best performance: 2x RTX PRO 6000 GPUs (or equivalent e.g. 2x RTX 5090) * Apple Vision Pro - * visionOS 2.0+ + * visionOS 26 * Apple M3 Pro chip with an 11-core CPU with at least 5 performance cores and 6 efficiency cores * 16GB unified memory * 256 GB SSD @@ -81,7 +81,8 @@ Prior to using CloudXR with Isaac Lab, please review the following system requir * Apple Silicon based Mac (for building the Isaac XR Teleop Sample Client App for Apple Vision Pro with Xcode) - * macOS Sonoma 14.5 or later + * macOS Sequoia 15.6 or later + * Xcode 26.0 * Wifi 6 capable router @@ -92,6 +93,10 @@ Prior to using CloudXR with Isaac Lab, please review the following system requir many institutional wireless networks will prevent devices from reaching each other, resulting in the Apple Vision Pro being unable to find the Isaac Lab workstation on the network) +.. note:: + If you are using DGX Spark, check `DGX Spark Limitations `_ for compatibility. + + .. _`Omniverse Spatial Streaming`: https://docs.omniverse.nvidia.com/avp/latest/setup-network.html @@ -167,6 +172,15 @@ There are two options to run the CloudXR Runtime Docker container: --files docker-compose.cloudxr-runtime.patch.yaml \ --env-file .env.cloudxr-runtime + .. tip:: + + If you encounter issues on restart, you can run the following command to clean up orphaned + containers: + + .. code:: bash + + docker system prune -f + .. dropdown:: Option 2: Run Isaac Lab as a local process and CloudXR Runtime container with Docker Isaac Lab can be run as a local process that connects to the CloudXR Runtime Docker container. @@ -198,12 +212,22 @@ There are two options to run the CloudXR Runtime Docker container: -p 48005:48005/udp \ -p 48008:48008/udp \ -p 48012:48012/udp \ - nvcr.io/nvidia/cloudxr-runtime:5.0.0 + nvcr.io/nvidia/cloudxr-runtime:5.0.1 .. note:: If you choose a particular GPU instead of ``all``, you need to make sure Isaac Lab also runs on that GPU. + .. tip:: + + If you encounter issues on running cloudxr-runtime container, you can run the following + command to clean up the orphaned container: + + .. code:: bash + + docker stop cloudxr-runtime + docker rm cloudxr-runtime + #. In a new terminal where you intend to run Isaac Lab, export the following environment variables, which reference the directory created above: @@ -225,13 +249,26 @@ There are two options to run the CloudXR Runtime Docker container: With Isaac Lab and the CloudXR Runtime running: -#. In the Isaac Sim UI: locate the Panel named **AR**. +#. In the Isaac Sim UI: locate the Panel named **AR** and choose the following options: + + * Selected Output Plugin: **OpenXR** + + * OpenXR Runtime: **System OpenXR Runtime** .. figure:: ../_static/setup/cloudxr_ar_panel.jpg :align: center :figwidth: 50% :alt: Isaac Sim UI: AR Panel + .. note:: + Isaac Sim lets you choose from several OpenXR runtime options: + + * **System OpenXR Runtime**: Use a runtime installed outside of Isaac Lab, such as the CloudXR Runtime set up via Docker in this tutorial. + + * **CloudXR Runtime (5.0)**: Use the built-in CloudXR Runtime. + + * **Custom**: Allow you to specify and run any custom OpenXR Runtime of your choice. + #. Click **Start AR**. The Viewport should show two eyes being rendered, and you should see the status "AR profile is @@ -273,19 +310,21 @@ On your Mac: git clone git@github.com:isaac-sim/isaac-xr-teleop-sample-client-apple.git -#. Check out the version tag corresponding to your Isaac Lab version: +#. Check out the App version that matches your Isaac Lab version: +-------------------+---------------------+ - | Isaac Lab Version | Client Version Tag | + | Isaac Lab Version | Client App Version | +-------------------+---------------------+ - | 2.2.x | v2.2.0 | + | 2.3 | v2.3.0 | +-------------------+---------------------+ - | 2.1.x | v1.0.0 | + | 2.2 | v2.2.0 | + +-------------------+---------------------+ + | 2.1 | v1.0.0 | +-------------------+---------------------+ .. code-block:: bash - git checkout + git checkout #. Follow the README in the repository to build and install the app on your Apple Vision Pro. @@ -298,6 +337,20 @@ Teleoperate an Isaac Lab Robot with Apple Vision Pro With the Isaac XR Teleop Sample Client installed on your Apple Vision Pro, you are ready to connect to Isaac Lab. +.. tip:: + + **Before wearing the headset**, you can first verify connectivity from your Mac: + + .. code:: bash + + # Test signaling port (replace with your workstation IP) + nc -vz 48010 + + Expected output: ``Connection to port 48010 [tcp/*] succeeded!`` + + If the connection fails, check that the runtime container is running (``docker ps``) and no stale + runtime container is blocking ports. + On your Isaac Lab workstation: #. Ensure that Isaac Lab and CloudXR are both running as described in @@ -378,7 +431,7 @@ Back on your Apple Vision Pro: .. note:: - The dots represent the tracked position of the hand joints. Latency or offset between the + The red dots represent the tracked position of the hand joints. Latency or offset between the motion of the dots and the robot may be caused by the limits of the robot joints and/or robot controller. @@ -409,10 +462,40 @@ Manus + Vive Hand Tracking Manus gloves and HTC Vive trackers can provide hand tracking when optical hand tracking from a headset is occluded. This setup expects Manus gloves with a Manus SDK license and Vive trackers attached to the gloves. -Requires Isaac Sim >=5.1. +Requires Isaac Sim 5.1 or later. Run the teleoperation example with Manus + Vive tracking: +.. dropdown:: Installation instructions + :open: + + Vive tracker integration is provided through the libsurvive library. + + To install, clone the repository, build the python package, and install the required udev rules. + In your Isaac Lab virtual environment, run the following commands: + + .. code-block:: bash + + git clone https://github.com/collabora/libsurvive.git + cd libsurvive + pip install scikit-build + python setup.py install + + sudo cp ./useful_files/81-vive.rules /etc/udev/rules.d/ + sudo udevadm control --reload-rules && sudo udevadm trigger + + + The Manus integration is provided through the Isaac Sim teleoperation input plugin framework. + Install the plugin by following the build and installation steps in `isaac-teleop-device-plugins `_. + +In the same terminal from which you will launch Isaac Lab, set: + +.. code-block:: bash + + export ISAACSIM_HANDTRACKER_LIB=/build-manus-default/lib/libIsaacSimManusHandTracking.so + +Once the plugin is installed, run the teleoperation example: + .. code-block:: bash ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ @@ -421,16 +504,37 @@ Run the teleoperation example with Manus + Vive tracking: --xr \ --enable_pinocchio -Begin the session with your palms facing up. -This is necessary for calibrating Vive tracker poses using Apple Vision Pro wrist poses from a few initial frames, -as the Vive trackers attached to the back of the hands occlude the optical hand tracking. +The recommended workflow, is to start Isaac Lab, click **Start AR**, and then put on the Manus gloves, vive trackers, and +headset. Once you are ready to begin the session, use voice commands to launch the Isaac XR teleop sample client and +connect to Isaac Lab. + +Isaac Lab automatically calibrates the Vive trackers using wrist pose data from the Apple Vision Pro during the initial +frames of the session. If calibration fails, for example, if the red dots do not accurately follow the teleoperator's +hands, restart Isaac Lab and begin with your hands in a palm-up position to improve calibration reliability. + +For optimal performance, position the lighthouse above the hands, tilted slightly downward. +Ensure the lighthouse remains stable; a stand is recommended to prevent wobbling. + +Ensure that while the task is being teleoperated, the hands remain stable and visible to the lighthouse at all times. +See: `Installing the Base Stations `_ +and `Tips for Setting Up the Base Stations `_ + +.. note:: + + On first launch of the Manus Vive device, the Vive lighthouses may take a few seconds to calibrate. Keep the Vive trackers + stable and visible to the lighthouse during this time. If the light houses are moved or if tracking fails or is unstable, + calibration can be forced by deleting the calibration file at: ``$XDG_RUNTIME_DIR/libsurvive/config.json``. If XDG_RUNTIME_DIR + is not set, the default directory is ``~/.config/libsurvive``. + + For more information consult the libsurvive documentation: `libsurvive `_. .. note:: To avoid resource contention and crashes, ensure Manus and Vive devices are connected to different USB controllers/buses. Use ``lsusb -t`` to identify different buses and connect devices accordingly. - Vive trackers are automatically calculated to map to the left and right wrist joints. + Vive trackers are automatically calculated to map to the left and right wrist joints obtained from a stable + OpenXR hand tracking wrist pose. This auto-mapping calculation supports up to 2 Vive trackers; if more than 2 Vive trackers are detected, it uses the first two trackers detected for calibration, which may not be correct. @@ -593,6 +697,12 @@ Isaac Lab provides three main retargeters for hand tracking: * Handles both left and right hands, converting hand poses to joint angles for the GR1T2 robot's hands * Supports visualization of tracked hand joints +.. dropdown:: UnitreeG1Retargeter (:class:`isaaclab.devices.openxr.retargeters.UnitreeG1Retargeter`) + + * Retargets OpenXR hand tracking data to Unitree G1 using Inspire 5-finger hand end-effector commands + * Handles both left and right hands, converting hand poses to joint angles for the G1 robot's hands + * Supports visualization of tracked hand joints + Retargeters can be combined to control different robot functions simultaneously. Using Retargeters with Hand Tracking @@ -632,6 +742,23 @@ Here's an example of setting up hand tracking: if terminated or truncated: break +Here's a diagram for the dataflow and algorithm used in humanoid teleoperation. Using Apple Vision Pro, we collect 26 keypoints for each hand. +The wrist keypoint is used to control the hand end-effector, while the remaining hand keypoints are used for hand retargeting. + +.. figure:: ../_static/teleop/teleop_diagram.jpg + :align: center + :figwidth: 80% + :alt: teleop_diagram + +For dex-retargeting, we are currently using the Dexpilot optimizer, which relies on the five fingertips and the palm for retargeting. It is essential +that the links used for retargeting are defined exactly at the fingertips—not in the middle of the fingers—to ensure accurate optimization.Please refer +to the image below for hand asset selection, find a suitable hand asset, or add fingertip links in IsaacLab as needed. + +.. figure:: ../_static/teleop/hand_asset.jpg + :align: center + :figwidth: 60% + :alt: hand_asset + .. _control-robot-with-xr-callbacks: Adding Callbacks for XR UI Events @@ -950,17 +1077,16 @@ You can create and register your own custom teleoperation devices by following t Known Issues ------------ -* ``[omni.kit.xr.system.openxr.plugin] Message received from CloudXR does not have a field called 'type'`` - - This error message can be safely ignored. It is caused by a deprecated, non-backwards-compatible - data message sent by the CloudXR Framework from Apple Vision Pro, and will be fixed in future - CloudXR Framework versions. - * ``XR_ERROR_VALIDATION_FAILURE: xrWaitFrame(frameState->type == 0)`` when stopping AR Mode This error message can be safely ignored. It is caused by a race condition in the exit handler for AR Mode. +* ``XR_ERROR_INSTANCE_LOST in xrPollEvent: Call to "xrt_session_poll_events" failed`` + + This error may occur if the CloudXR runtime exits before Isaac Lab. Restart the CloudXR + runtime to resume teleoperation. + * ``[omni.usd] TF_PYTHON_EXCEPTION`` when starting/stopping AR Mode This error message can be safely ignored. It is caused by a race condition in the enter/exit @@ -971,6 +1097,13 @@ Known Issues This error message can be caused by shader assets authored with older versions of USD, and can typically be ignored. +* The XR device connects successfully, but no video is displayed, even though the Isaac Lab viewport responds to tracking. + + This error occurs when the GPU index differs between the host and the container, causing CUDA + to load on the wrong GPU. To fix this, set ``NV_GPU_INDEX`` in the runtime container to ``0``, ``1``, + or ``2`` to ensure the GPU selected by CUDA matches the host. + + Kubernetes Deployment --------------------- diff --git a/docs/source/how-to/import_new_asset.rst b/docs/source/how-to/import_new_asset.rst index 9d2f828ad40..41eacc48673 100644 --- a/docs/source/how-to/import_new_asset.rst +++ b/docs/source/how-to/import_new_asset.rst @@ -307,8 +307,8 @@ of gravity. .. _instanceable: https://openusd.org/dev/api/_usd__page__scenegraph_instancing.html .. _documentation: https://docs.isaacsim.omniverse.nvidia.com/latest/isaac_lab_tutorials/tutorial_instanceable_assets.html -.. _MJCF importer: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/import_mjcf.html -.. _URDF importer: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/import_urdf.html +.. _MJCF importer: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_mjcf.html +.. _URDF importer: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_urdf.html .. _anymal.urdf: https://github.com/isaac-orbit/anymal_d_simple_description/blob/master/urdf/anymal.urdf .. _asset converter: https://docs.omniverse.nvidia.com/extensions/latest/ext_asset-converter.html .. _mujoco_menagerie: https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_h1 diff --git a/docs/source/how-to/master_omniverse.rst b/docs/source/how-to/master_omniverse.rst index 0108ab64821..ee3e0d55c4e 100644 --- a/docs/source/how-to/master_omniverse.rst +++ b/docs/source/how-to/master_omniverse.rst @@ -68,7 +68,7 @@ Importing assets - `Omniverse Create - Importing FBX Files \| NVIDIA Omniverse Tutorials `__ - `Omniverse Asset Importer `__ -- `Isaac Sim URDF impoter `__ +- `Isaac Sim URDF impoter `__ Part 2: Scripting in Omniverse @@ -119,6 +119,5 @@ Part 3: More Resources - `Omniverse Glossary of Terms `__ - `Omniverse Code Samples `__ -- `PhysX Collider Compatibility `__ - `PhysX Limitations `__ - `PhysX Documentation `__. diff --git a/docs/source/how-to/simulation_performance.rst b/docs/source/how-to/simulation_performance.rst index ec575685b00..3dd113a1285 100644 --- a/docs/source/how-to/simulation_performance.rst +++ b/docs/source/how-to/simulation_performance.rst @@ -1,5 +1,5 @@ -Simulation Performance -======================= +Simulation Performance and Tuning +==================================== The performance of the simulation can be affected by various factors, including the number of objects in the scene, the complexity of the physics simulation, and the hardware being used. Here are some tips to improve performance: @@ -43,8 +43,30 @@ collision detection will fall back to CPU. Collisions with particles and deforma Suitable workarounds include switching to a bounding cube approximation, or using a static triangle mesh collider if the geometry is not part of a dynamic rigid body. +CPU Governor Settings on Linux +------------------------------ + +CPU governors dictate the operating clock frequency range and scaling of the CPU. This can be a limiting factor for Isaac Sim performance. For maximum performance, the CPU governor should be set to ``performance``. To modify the CPU governor, run the following commands: + +.. code-block:: bash + + sudo apt-get install linux-tools-common + cpupower frequency-info # Check available governors + sudo cpupower frequency-set -g performance # Set governor with root permissions + +.. note:: + + Not all governors are available on all systems. Governors enabling higher clock speed are typically more performance-centric and will yield better performance for Isaac Sim. + Additional Performance Guides ----------------------------- +There are many ways to "tune" the performance of the simulation, but the way you choose largely depends on what you are trying to simulate. In general, the first place +you will want to look for performance gains is with the `physics engine `_. Next to rendering +and running deep learning models, the physics engine is the most computationally costly. Tuning the physics sim to limit the scope to only the task of interest is a great place to +start hunting for performance gains. + +We have recently released a new `gripper tuning guide `_ , specific to contact and grasp tuning. Please check it first if you intend to use robot grippers. For additional details, you should also checkout these guides! + * `Isaac Sim Performance Optimization Handbook `_ * `Omni Physics Simulation Performance Guide `_ diff --git a/docs/source/overview/developer-guide/vs_code.rst b/docs/source/overview/developer-guide/vs_code.rst index f9ea07b6da3..e0c0040cffa 100644 --- a/docs/source/overview/developer-guide/vs_code.rst +++ b/docs/source/overview/developer-guide/vs_code.rst @@ -52,8 +52,7 @@ If everything executes correctly, it should create the following files: For more information on VSCode support for Omniverse, please refer to the following links: -* `Isaac Sim VSCode support `__ -* `Debugging with VSCode `__ +* `Isaac Sim VSCode support `__ Configuring the python interpreter diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index c4925adfb94..10e90e2658c 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -100,51 +100,69 @@ for the lift-cube environment: .. table:: :widths: 33 37 30 - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | World | Environment ID | Description | - +======================+===========================+=============================================================================+ - | |reach-franka| | |reach-franka-link| | Move the end-effector to a sampled target pose with the Franka robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |reach-ur10| | |reach-ur10-link| | Move the end-effector to a sampled target pose with the UR10 robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |deploy-reach-ur10e| | |deploy-reach-ur10e-link| | Move the end-effector to a sampled target pose with the UR10e robot | - | | | This policy has been deployed to a real robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |lift-cube| | |lift-cube-link| | Pick a cube and bring it to a sampled target position with the Franka robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |stack-cube| | |stack-cube-link| | Stack three cubes (bottom to top: blue, red, green) with the Franka robot. | - | | | Blueprint env used for the NVIDIA Isaac GR00T blueprint for synthetic | - | | |stack-cube-bp-link| | manipulation motion generation | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |surface-gripper| | |long-suction-link| | Stack three cubes (bottom to top: blue, red, green) | - | | | with the UR10 arm and long surface gripper | - | | |short-suction-link| | or short surface gripper. | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |cabi-franka| | |cabi-franka-link| | Grasp the handle of a cabinet's drawer and open it with the Franka robot | - | | | | - | | |franka-direct-link| | | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |cube-allegro| | |cube-allegro-link| | In-hand reorientation of a cube using Allegro hand | - | | | | - | | |allegro-direct-link| | | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |cube-shadow| | |cube-shadow-link| | In-hand reorientation of a cube using Shadow hand | - | | | | - | | |cube-shadow-ff-link| | | - | | | | - | | |cube-shadow-lstm-link| | | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |cube-shadow| | |cube-shadow-vis-link| | In-hand reorientation of a cube using Shadow hand using perceptive inputs. | - | | | Requires running with ``--enable_cameras``. | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |gr1_pick_place| | |gr1_pick_place-link| | Pick up and place an object in a basket with a GR-1 humanoid robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |gr1_pp_waist| | |gr1_pp_waist-link| | Pick up and place an object in a basket with a GR-1 humanoid robot | - | | | with waist degrees-of-freedom enables that provides a wider reach space. | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ - | |galbot_stack| | |galbot_stack-link| | Stack three cubes (bottom to top: blue, red, green) with the left arm of | - | | | a Galbot humanoid robot | - +----------------------+---------------------------+-----------------------------------------------------------------------------+ + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | World | Environment ID | Description | + +=========================+==============================+=============================================================================+ + | |reach-franka| | |reach-franka-link| | Move the end-effector to a sampled target pose with the Franka robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |reach-ur10| | |reach-ur10-link| | Move the end-effector to a sampled target pose with the UR10 robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |deploy-reach-ur10e| | |deploy-reach-ur10e-link| | Move the end-effector to a sampled target pose with the UR10e robot | + | | | This policy has been deployed to a real robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |lift-cube| | |lift-cube-link| | Pick a cube and bring it to a sampled target position with the Franka robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |stack-cube| | |stack-cube-link| | Stack three cubes (bottom to top: blue, red, green) with the Franka robot. | + | | | Blueprint env used for the NVIDIA Isaac GR00T blueprint for synthetic | + | | |stack-cube-bp-link| | manipulation motion generation | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |surface-gripper| | |long-suction-link| | Stack three cubes (bottom to top: blue, red, green) | + | | | with the UR10 arm and long surface gripper | + | | |short-suction-link| | or short surface gripper. | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |cabi-franka| | |cabi-franka-link| | Grasp the handle of a cabinet's drawer and open it with the Franka robot | + | | | | + | | |franka-direct-link| | | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |cube-allegro| | |cube-allegro-link| | In-hand reorientation of a cube using Allegro hand | + | | | | + | | |allegro-direct-link| | | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |cube-shadow| | |cube-shadow-link| | In-hand reorientation of a cube using Shadow hand | + | | | | + | | |cube-shadow-ff-link| | | + | | | | + | | |cube-shadow-lstm-link| | | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |cube-shadow| | |cube-shadow-vis-link| | In-hand reorientation of a cube using Shadow hand using perceptive inputs. | + | | | Requires running with ``--enable_cameras``. | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |gr1_pick_place| | |gr1_pick_place-link| | Pick up and place an object in a basket with a GR-1 humanoid robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |gr1_pp_waist| | |gr1_pp_waist-link| | Pick up and place an object in a basket with a GR-1 humanoid robot | + | | | with waist degrees-of-freedom enables that provides a wider reach space. | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |g1_pick_place| | |g1_pick_place-link| | Pick up and place an object in a basket with a Unitree G1 humanoid robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |g1_pick_place_fixed| | |g1_pick_place_fixed-link| | Pick up and place an object in a basket with a Unitree G1 humanoid robot | + | | | with three-fingered hands. Robot is set up with the base fixed in place. | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |g1_pick_place_lm| | |g1_pick_place_lm-link| | Pick up and place an object in a basket with a Unitree G1 humanoid robot | + | | | with three-fingered hands and in-place locomanipulation capabilities | + | | | enabled (i.e. Robot lower body balances in-place while upper body is | + | | | controlled via Inverse Kinematics). | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |kuka-allegro-lift| | |kuka-allegro-lift-link| | Pick up a primitive shape on the table and lift it to target position | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |kuka-allegro-reorient| | |kuka-allegro-reorient-link| | Pick up a primitive shape on the table and orient it to target pose | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |galbot_stack| | |galbot_stack-link| | Stack three cubes (bottom to top: blue, red, green) with the left arm of | + | | | a Galbot humanoid robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |agibot_place_mug| | |agibot_place_mug-link| | Pick up and place a mug upright with a Agibot A2D humanoid robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ + | |agibot_place_toy| | |agibot_place_toy-link| | Pick up and place an object in a box with a Agibot A2D humanoid robot | + +-------------------------+------------------------------+-----------------------------------------------------------------------------+ .. |reach-franka| image:: ../_static/tasks/manipulation/franka_reach.jpg .. |reach-ur10| image:: ../_static/tasks/manipulation/ur10_reach.jpg @@ -155,10 +173,16 @@ for the lift-cube environment: .. |cube-shadow| image:: ../_static/tasks/manipulation/shadow_cube.jpg .. |stack-cube| image:: ../_static/tasks/manipulation/franka_stack.jpg .. |gr1_pick_place| image:: ../_static/tasks/manipulation/gr-1_pick_place.jpg +.. |g1_pick_place| image:: ../_static/tasks/manipulation/g1_pick_place.jpg +.. |g1_pick_place_fixed| image:: ../_static/tasks/manipulation/g1_pick_place_fixed_base.jpg +.. |g1_pick_place_lm| image:: ../_static/tasks/manipulation/g1_pick_place_locomanipulation.jpg .. |surface-gripper| image:: ../_static/tasks/manipulation/ur10_stack_surface_gripper.jpg .. |gr1_pp_waist| image:: ../_static/tasks/manipulation/gr-1_pick_place_waist.jpg -.. |surface-gripper| image:: ../_static/tasks/manipulation/ur10_stack_surface_gripper.jpg .. |galbot_stack| image:: ../_static/tasks/manipulation/galbot_stack_cube.jpg +.. |agibot_place_mug| image:: ../_static/tasks/manipulation/agibot_place_mug.jpg +.. |agibot_place_toy| image:: ../_static/tasks/manipulation/agibot_place_toy.jpg +.. |kuka-allegro-lift| image:: ../_static/tasks/manipulation/kuka_allegro_lift.jpg +.. |kuka-allegro-reorient| image:: ../_static/tasks/manipulation/kuka_allegro_reorient.jpg .. |reach-franka-link| replace:: `Isaac-Reach-Franka-v0 `__ .. |reach-ur10-link| replace:: `Isaac-Reach-UR10-v0 `__ @@ -173,18 +197,23 @@ for the lift-cube environment: .. |stack-cube-link| replace:: `Isaac-Stack-Cube-Franka-v0 `__ .. |stack-cube-bp-link| replace:: `Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0 `__ .. |gr1_pick_place-link| replace:: `Isaac-PickPlace-GR1T2-Abs-v0 `__ +.. |g1_pick_place-link| replace:: `Isaac-PickPlace-G1-InspireFTP-Abs-v0 `__ +.. |g1_pick_place_fixed-link| replace:: `Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0 `__ +.. |g1_pick_place_lm-link| replace:: `Isaac-PickPlace-Locomanipulation-G1-Abs-v0 `__ .. |long-suction-link| replace:: `Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0 `__ .. |short-suction-link| replace:: `Isaac-Stack-Cube-UR10-Short-Suction-IK-Rel-v0 `__ .. |gr1_pp_waist-link| replace:: `Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0 `__ .. |galbot_stack-link| replace:: `Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-v0 `__ -.. |long-suction-link| replace:: `Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0 `__ -.. |short-suction-link| replace:: `Isaac-Stack-Cube-UR10-Short-Suction-IK-Rel-v0 `__ - +.. |kuka-allegro-lift-link| replace:: `Isaac-Dexsuite-Kuka-Allegro-Lift-v0 `__ +.. |kuka-allegro-reorient-link| replace:: `Isaac-Dexsuite-Kuka-Allegro-Reorient-v0 `__ .. |cube-shadow-link| replace:: `Isaac-Repose-Cube-Shadow-Direct-v0 `__ .. |cube-shadow-ff-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0 `__ .. |cube-shadow-lstm-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0 `__ .. |cube-shadow-vis-link| replace:: `Isaac-Repose-Cube-Shadow-Vision-Direct-v0 `__ +.. |agibot_place_mug-link| replace:: `Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0 `__ +.. |agibot_place_toy-link| replace:: `Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 `__ + Contact-rich Manipulation ~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -231,13 +260,12 @@ We provide environments for both disassembly and assembly. .. attention:: - CUDA is required for running the AutoMate environments. - Follow the below steps to install CUDA 12.8: + CUDA is recommended for running the AutoMate environments with 570 drivers. If running with Nvidia driver 570 on Linux with architecture x86_64, we follow the below steps to install CUDA 12.8. This allows for computing rewards in AutoMate environments with CUDA. If you have a different operation system or architecture, please refer to the `CUDA installation page `_ for additional instruction. .. code-block:: bash wget https://developer.download.nvidia.com/compute/cuda/12.8.0/local_installers/cuda_12.8.0_570.86.10_linux.run - sudo sh cuda_12.8.0_570.86.10_linux.run + sudo sh cuda_12.8.0_570.86.10_linux.run --toolkit When using conda, cuda toolkit can be installed with: @@ -245,7 +273,7 @@ We provide environments for both disassembly and assembly. conda install cudatoolkit - For addition instructions and Windows installation, please refer to the `CUDA installation page `_. + With 580 drivers and CUDA 13, we are currently unable to enable CUDA for computing the rewards. The code automatically fallbacks to CPU, resulting in slightly slower performance. * |disassembly-link|: The plug starts inserted in the socket. A low-level controller lifts the plug out and moves it to a random position. This process is purely scripted and does not involve any learned policy. Therefore, it does not require policy training or evaluation. The resulting trajectories serve as demonstrations for the reverse process, i.e., learning to assemble. To run disassembly for a specific task: ``python source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_disassembly_w_id.py --assembly_id=ASSEMBLY_ID --disassembly_dir=DISASSEMBLY_DIR``. All generated trajectories are saved to a local directory ``DISASSEMBLY_DIR``. * |assembly-link|: The goal is to insert the plug into the socket. You can use this environment to train a policy via reinforcement learning or evaluate a pre-trained checkpoint. @@ -942,6 +970,14 @@ inferencing, including reading from an already trained checkpoint and disabling - - Manager Based - + * - Isaac-Dexsuite-Kuka-Allegro-Lift-v0 + - Isaac-Dexsuite-Kuka-Allegro-Lift-Play-v0 + - Manager Based + - **rl_games** (PPO), **rsl_rl** (PPO) + * - Isaac-Dexsuite-Kuka-Allegro-Reorient-v0 + - Isaac-Dexsuite-Kuka-Allegro-Reorient-Play-v0 + - Manager Based + - **rl_games** (PPO), **rsl_rl** (PPO) * - Isaac-Stack-Cube-Franka-v0 - - Manager Based @@ -954,6 +990,10 @@ inferencing, including reading from an already trained checkpoint and disabling - - Manager Based - + * - Isaac-PickPlace-G1-InspireFTP-Abs-v0 + - + - Manager Based + - * - Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0 - - Manager Based @@ -974,6 +1014,35 @@ inferencing, including reading from an already trained checkpoint and disabling - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Play-v0 - Manager Based - + * - Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-v0 + - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Play-v0 + - Manager Based + - + * - Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 + - + - Manager Based + - + * - Isaac-Velocity-Flat-Anymal-B-v0 - Isaac-Velocity-Flat-Anymal-B-Play-v0 - Manager Based diff --git a/docs/source/overview/imitation-learning/augmented_imitation.rst b/docs/source/overview/imitation-learning/augmented_imitation.rst index 38059879c71..b3593f22e62 100644 --- a/docs/source/overview/imitation-learning/augmented_imitation.rst +++ b/docs/source/overview/imitation-learning/augmented_imitation.rst @@ -80,6 +80,8 @@ Example usage for the cube stacking task: --input_file datasets/mimic_dataset_1k.hdf5 \ --output_dir datasets/mimic_dataset_1k_mp4 +.. _running-cosmos: + Running Cosmos for Visual Augmentation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -101,6 +103,9 @@ We provide an example augmentation output from `Cosmos Transfer1 `_ model for visual augmentation as we found it to produce the best results in the form of a highly diverse dataset with a wide range of visual variations. You can refer to the `installation instructions `_, the `checkpoint download instructions `_ and `this example `_ for reference on how to use Transfer1 for this usecase. We further recommend the following settings to be used with the Transfer1 model for this task: +.. note:: + This workflow has been tested with commit ``e4055e39ee9c53165e85275bdab84ed20909714a`` of the Cosmos Transfer1 repository, and it is the recommended version to use. After cloning the Cosmos Transfer1 repository, checkout to this specific commit by running ``git checkout e4055e39ee9c53165e85275bdab84ed20909714a``. + .. rubric:: Hyperparameters .. list-table:: diff --git a/docs/source/overview/imitation-learning/index.rst b/docs/source/overview/imitation-learning/index.rst index 1daf0968fac..f8f77d031fb 100644 --- a/docs/source/overview/imitation-learning/index.rst +++ b/docs/source/overview/imitation-learning/index.rst @@ -7,6 +7,6 @@ with Isaac Lab. .. toctree:: :maxdepth: 1 - augmented_imitation teleop_imitation + augmented_imitation skillgen diff --git a/docs/source/overview/imitation-learning/skillgen.rst b/docs/source/overview/imitation-learning/skillgen.rst index 28d2dbe5805..b577f82e13a 100644 --- a/docs/source/overview/imitation-learning/skillgen.rst +++ b/docs/source/overview/imitation-learning/skillgen.rst @@ -61,6 +61,24 @@ cuRobo provides the motion planning capabilities for SkillGen. This installation * cuRobo is installed from source and is editable installed. This means that the cuRobo source code will be cloned in the current directory under ``src/nvidia-curobo``. Users can choose their working directory to install cuRobo. + * ``TORCH_CUDA_ARCH_LIST`` in the above command should match your GPU's CUDA compute capability (e.g., ``8.0`` for A100, ``8.6`` for many RTX 30‑series, ``8.9`` for RTX 4090); the ``+PTX`` suffix embeds PTX for forward compatibility so newer GPUs can JIT‑compile when native SASS isn’t included. + +.. warning:: + + **cuRobo installation may fail if Isaac Sim environment scripts are sourced** + + Sourcing Omniverse Kit/Isaac Sim environment scripts (for example, ``setup_conda_env.sh``) exports ``PYTHONHOME`` and ``PYTHONPATH`` to the Kit runtime and its pre-bundled Python packages. During cuRobo installation this can cause ``conda`` to import Omniverse's bundled libraries (e.g., ``requests``/``urllib3``) before initialization, resulting in a crash (often seen as a ``TypeError`` referencing ``omni.kit.pip_archive``). + + Do one of the following: + + - Install cuRobo from a clean shell that has not sourced any Omniverse/Isaac Sim scripts. + - Temporarily reset or ignore inherited Python environment variables (notably ``PYTHONPATH`` and ``PYTHONHOME``) before invoking Conda, so Kit's Python does not shadow your Conda environment. + - Use Conda mechanisms that do not rely on shell activation and avoid inheriting the current shell's Python variables. + + After installation completes, you may source Isaac Lab/Isaac Sim scripts again for normal use. + + + Step 3: Install Rerun ^^^^^^^^^^^^^^^^^^^^^ @@ -112,7 +130,7 @@ The dataset contains: * Human demonstrations of Franka arm cube stacking * Manually annotated subtask boundaries for each demonstration -* Compatible with both basic cube stacking and adaptive bin stacking tasks +* Compatible with both basic cube stacking and adaptive bin cube stacking tasks Download and Setup ^^^^^^^^^^^^^^^^^^ @@ -124,7 +142,7 @@ Download and Setup .. code:: bash # Make sure you are in the root directory of your Isaac Lab workspace - cd /path/to/your/isaaclab/root + cd /path/to/your/IsaacLab # Create the datasets directory if it does not exist mkdir -p datasets @@ -232,6 +250,8 @@ Key parameters for SkillGen data generation: * ``--device``: Computation device (cpu/cuda). Use cpu for stable physics * ``--headless``: Disable visualization for faster generation +.. _task-basic-cube-stacking: + Task 1: Basic Cube Stacking ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -280,16 +300,17 @@ Once satisfied with small-scale results, generate a full training dataset: --input_file ./datasets/annotated_dataset_skillgen.hdf5 \ --output_file ./datasets/generated_dataset_skillgen_cube_stack.hdf5 \ --task Isaac-Stack-Cube-Franka-IK-Rel-Skillgen-v0 \ - --use_skillgen \ - --headless + --use_skillgen .. note:: * Use ``--headless`` to disable visualization for faster generation. Rerun visualization can be enabled by setting ``visualize_plan = True`` in the cuRobo planner configuration with ``--headless`` enabled as well for debugging. * Adjust ``--num_envs`` based on your GPU memory (start with 1, increase gradually). The performance gain is not very significant when num_envs is greater than 1. A value of 5 seems to be a sweet spot for most GPUs to balance performance and memory usage between cuRobo instances and simulation environments. - * Generation time: ~90 to 120 minutes for one environment for 1000 demonstrations on modern GPUs. Time depends on the GPU, the number of environments, and the success rate of the demonstrations (which depends on quality of the annotated dataset). + * Generation time: ~90 to 120 minutes for one environment with ``--headless`` enabled for 1000 demonstrations on a RTX 6000 Ada GPU. Time depends on the GPU, the number of environments, and the success rate of the demonstrations (which depends on quality of the annotated dataset). * cuRobo planner interface and configurations are described in :ref:`cuRobo-interface-features`. +.. _task-bin-cube-stacking: + Task 2: Adaptive Cube Stacking in a Bin ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ SkillGen can also be used to generate datasets for adaptive tasks. In this example, we generate a dataset for adaptive cube stacking in a narrow bin. The bin is placed at a fixed position and orientation in the workspace and a blue cube is placed at the center of the bin. The robot must generate successful demonstrations for stacking the red and green cubes on the blue cube without colliding with the bin. @@ -339,6 +360,19 @@ Generate the complete adaptive stacking dataset: Adaptive tasks typically have lower success rates and higher data generation time due to increased complexity. The time taken to generate the dataset is also longer due to lower success rates than vanilla cube stacking and difficult planning problems. +.. note:: + + If the pre-annotated dataset is used and the data generation command is run with ``--headless`` enabled, the generation time is typically around ~220 minutes for 1000 demonstrations for a single environment on a RTX 6000 Ada GPU. + +.. note:: + + **VRAM usage and GPU recommendations** + + Figures measured over 10 generated demonstrations on an RTX 6000 Ada. + * Vanilla Cube Stacking: 1 env ~9.3–9.6 GB steady; 5 envs ~21.8–22.2 GB steady (briefly higher during initialization). + * Adaptive Bin Cube Stacking: 1 env ~9.3–9.6 GB steady; 5 envs ~22.0–22.3 GB steady (briefly higher during initialization). + * Minimum recommended GPU: ≥24 GB VRAM for ``--num_envs`` 1–2; ≥48 GB VRAM for ``--num_envs`` up to ~5. + * To reduce VRAM: prefer ``--headless`` and keep ``--num_envs`` modest. Numbers can vary with scene assets and number of demonstrations. Learning Policies from SkillGen Data ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -357,8 +391,8 @@ Train a state-based policy for the basic cube stacking task: --algo bc \ --dataset ./datasets/generated_dataset_skillgen_cube_stack.hdf5 -Adaptive Bin Stacking Policy -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Adaptive Bin Cube Stacking Policy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Train a policy for the more complex adaptive bin stacking: @@ -374,7 +408,7 @@ Train a policy for the more complex adaptive bin stacking: The training script will save the model checkpoints in the model directory under ``IssacLab/logs/robomimic``. Evaluating Trained Policies -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^ Test your trained policies: @@ -389,13 +423,24 @@ Test your trained policies: .. code:: bash - # Adaptive bin stacking evaluation + # Adaptive bin cube stacking evaluation ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ --device cpu \ --task Isaac-Stack-Cube-Bin-Franka-IK-Rel-Mimic-v0 \ --num_rollouts 50 \ --checkpoint /path/to/model_checkpoint.pth +.. note:: + + **Expected Success Rates and Recommendations for Cube Stacking and Bin Cube Stacking Tasks** + + * SkillGen data generation and downstream policy success are sensitive to the task and the quality of dataset annotation, and can show high variance. + * For cube stacking and bin cube stacking, data generation success is typically 40% to 70% when the dataset is properly annotated per the instructions. + * Behavior Cloning (BC) policy success from 1000 generated demonstrations trained for 2000 epochs (default) is typically 40% to 85% for these tasks, depending on data quality. + * Training the policy with 1000 demonstrations and for 2000 epochs takes about 30 to 35 minutes on a RTX 6000 Ada GPU. Training time increases with the number of demonstrations and epochs. + * For dataset generation time, see :ref:`task-basic-cube-stacking` and :ref:`task-bin-cube-stacking`. + * Recommendation: Train for the default 2000 epochs with about 1000 generated demonstrations, and evaluate multiple checkpoints saved after the 1000th epoch to select the best-performing policy. + .. _cuRobo-interface-features: cuRobo Interface Features @@ -416,9 +461,9 @@ cuRobo Planner (GPU, collision-aware) * Location: ``isaaclab_mimic/motion_planners/curobo`` * Multi-phase planning: - * Approach → Contact → Retreat phases per subtask + * Retreat → Contact → Approach phases per subtask * Configurable collision filtering in contact phases - * For SkillGen, approach and retreat phases are collision-free. The transit phase is collision-checked. + * For SkillGen, retreat and approach phases are collision-free. The transit phase is collision-checked. * World synchronization: diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 859287560a8..d7d1b4d0a46 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -61,7 +61,7 @@ For tasks that benefit from the use of an extended reality (XR) device with hand .. note:: - See :ref:`cloudxr-teleoperation` to learn more about using CloudXR with Isaac Lab. + See :ref:`cloudxr-teleoperation` to learn how to use CloudXR and experience teleoperation with Isaac Lab. The script prints the teleoperation events configured. For keyboard, @@ -140,12 +140,14 @@ Pre-recorded demonstrations ^^^^^^^^^^^^^^^^^^^^^^^^^^^ We provide a pre-recorded ``dataset.hdf5`` containing 10 human demonstrations for ``Isaac-Stack-Cube-Franka-IK-Rel-v0`` -`here `__. +here: `[Franka Dataset] `__. This dataset may be downloaded and used in the remaining tutorial steps if you do not wish to collect your own demonstrations. .. note:: Use of the pre-recorded dataset is optional. +.. _generating-additional-demonstrations: + Generating additional demonstrations with Isaac Lab Mimic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -156,6 +158,9 @@ Isaac Lab Mimic is a feature in Isaac Lab that allows generation of additional d In the following example, we will show how to use Isaac Lab Mimic to generate additional demonstrations that can be used to train either a state-based policy (using the ``Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0`` environment) or visuomotor policy (using the ``Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Mimic-v0`` environment). +.. note:: + The following commands are run using CPU mode as a small number of envs are used which are I/O bound rather than compute bound. + .. important:: All commands in the following sections must keep a consistent policy type. For example, if choosing to use a state-based policy, then all commands used should be from the "State-based policy" tab. @@ -310,6 +315,15 @@ By inferencing using the generated model, we can visualize the results of the po --device cpu --enable_cameras --task Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0 --num_rollouts 50 \ --checkpoint /PATH/TO/desired_model_checkpoint.pth +.. note:: + + **Expected Success Rates and Timings for Franka Cube Stack Task** + + * Data generation success rate: ~50% (for both state + visuomotor) + * Data generation time: ~30 mins for state, ~4 hours for visuomotor (varies based on num envs the user runs) + * BC RNN training time: 1000 epochs + ~30 mins (for state), 600 epochs + ~6 hours (for visuomotor) + * BC RNN policy success rate: ~40-60% (for both state + visuomotor) + Demo 1: Data Generation and Policy Training for a Humanoid Robot ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -451,7 +465,7 @@ Generate the dataset ^^^^^^^^^^^^^^^^^^^^ If you skipped the prior collection and annotation step, download the pre-recorded annotated dataset ``dataset_annotated_gr1.hdf5`` from -`here `__. +here: `[Annotated GR1 Dataset] `_. Place the file under ``IsaacLab/datasets`` and run the following command to generate a new dataset with 1000 demonstrations. .. code:: bash @@ -507,15 +521,226 @@ Visualize the results of the trained policy by running the following command, us The trained policy performing the pick and place task in Isaac Lab. +.. note:: + + **Expected Success Rates and Timings for Pick and Place GR1T2 Task** + + * Success rate for data generation depends on the quality of human demonstrations (how well the user performs them) and dataset annotation quality. Both data generation and downstream policy success are sensitive to these factors and can show high variance. See :ref:`Common Pitfalls when Generating Data ` for tips to improve your dataset. + * Data generation success for this task is typically 65-80% over 1000 demonstrations, taking 18-40 minutes depending on GPU hardware and success rate (19 minutes on a RTX ADA 6000 @ 80% success rate). + * Behavior Cloning (BC) policy success is typically 75-86% (evaluated on 50 rollouts) when trained on 1000 generated demonstrations for 2000 epochs (default), depending on demonstration quality. Training takes approximately 29 minutes on a RTX ADA 6000. + * Recommendation: Train for 2000 epochs with 1000 generated demonstrations, and evaluate multiple checkpoints saved between the 1500th and 2000th epochs to select the best-performing policy. + + +Demo 2: Data Generation and Policy Training for Humanoid Robot Locomanipulation with Unitree G1 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +In this demo, we showcase the integration of locomotion and manipulation capabilities within a single humanoid robot system. +This locomanipulation environment enables data collection for complex tasks that combine navigation and object manipulation. +The demonstration follows a multi-step process: first, it generates pick and place tasks similar to Demo 1, then introduces +a navigation component that uses specialized scripts to generate scenes where the humanoid robot must move from point A to point B. +The robot picks up an object at the initial location (point A) and places it at the target destination (point B). + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation-g-1_steering_wheel_pick_place.gif + :width: 100% + :align: center + :alt: G1 humanoid robot with locomanipulation performing a pick and place task + :figclass: align-center + +Generate the manipulation dataset +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The same data generation and policy training steps from Demo 1.0 can be applied to the G1 humanoid robot with locomanipulation capabilities. +This demonstration shows how to train a G1 robot to perform pick and place tasks with full-body locomotion and manipulation. + +The process follows the same workflow as Demo 1.0, but uses the ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0`` task environment. + +Follow the same data collection, annotation, and generation process as demonstrated in Demo 1.0, but adapted for the G1 locomanipulation task. + +.. hint:: + + If desired, data collection and annotation can be done using the same commands as the prior examples for validation of the dataset. + + The G1 robot with locomanipulation capabilities combines full-body locomotion with manipulation to perform pick and place tasks. + + **Note that the following commands are only for your reference and dataset validation purposes - they are not required for this demo.** + + To collect demonstrations: + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --device cpu \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --teleop_device handtracking \ + --dataset_file ./datasets/dataset_g1_locomanip.hdf5 \ + --num_demos 5 --enable_pinocchio + + .. note:: + + Depending on how the Apple Vision Pro app was initialized, the hands of the operator might be very far up or far down compared to the hands of the G1 robot. If this is the case, you can click **Stop AR** in the AR tab in Isaac Lab, and move the AR Anchor prim. Adjust it down to bring the hands of the operator lower, and up to bring them higher. Click **Start AR** to resume teleoperation session. Make sure to match the hands of the robot before clicking **Play** in the Apple Vision Pro, otherwise there will be an undesired large force generated initially. + + You can replay the collected demonstrations by running: + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/replay_demos.py \ + --device cpu \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --dataset_file ./datasets/dataset_g1_locomanip.hdf5 --enable_pinocchio + + To annotate the demonstrations: + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu \ + --task Isaac-Locomanipulation-G1-Abs-Mimic-v0 \ + --input_file ./datasets/dataset_g1_locomanip.hdf5 \ + --output_file ./datasets/dataset_annotated_g1_locomanip.hdf5 --enable_pinocchio -Demo 2: Visuomotor Policy for a Humanoid Robot + +If you skipped the prior collection and annotation step, download the pre-recorded annotated dataset ``dataset_annotated_g1_locomanip.hdf5`` from +here: `[Annotated G1 Dataset] `_. +Place the file under ``IsaacLab/datasets`` and run the following command to generate a new dataset with 1000 demonstrations. + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --headless --num_envs 20 --generation_num_trials 1000 --enable_pinocchio \ + --input_file ./datasets/dataset_annotated_g1_locomanip.hdf5 --output_file ./datasets/generated_dataset_g1_locomanip.hdf5 + + +Train a manipulation-only policy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +At this point you can train a policy that only performs manipulation tasks using the generated dataset: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 --algo bc \ + --normalize_training_actions \ + --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 + +Visualize the results +^^^^^^^^^^^^^^^^^^^^^ + +Visualize the trained policy performance: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu \ + --enable_pinocchio \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --num_rollouts 50 \ + --horizon 400 \ + --norm_factor_min \ + --norm_factor_max \ + --checkpoint /PATH/TO/desired_model_checkpoint.pth + +.. note:: + Change the ``NORM_FACTOR`` in the above command with the values generated in the training step. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation-g-1_steering_wheel_pick_place.gif + :width: 100% + :align: center + :alt: G1 humanoid robot performing a pick and place task + :figclass: align-center + + The trained policy performing the pick and place task in Isaac Lab. + +.. note:: + + **Expected Success Rates and Timings for Locomanipulation Pick and Place Task** + + * Success rate for data generation depends on the quality of human demonstrations (how well the user performs them) and dataset annotation quality. Both data generation and downstream policy success are sensitive to these factors and can show high variance. See :ref:`Common Pitfalls when Generating Data ` for tips to improve your dataset. + * Data generation success for this task is typically 65-82% over 1000 demonstrations, taking 18-40 minutes depending on GPU hardware and success rate (18 minutes on a RTX ADA 6000 @ 82% success rate). + * Behavior Cloning (BC) policy success is typically 75-85% (evaluated on 50 rollouts) when trained on 1000 generated demonstrations for 2000 epochs (default), depending on demonstration quality. Training takes approximately 40 minutes on a RTX ADA 6000. + * Recommendation: Train for 2000 epochs with 1000 generated demonstrations, and evaluate multiple checkpoints saved between the 1500th and 2000th epochs to select the best-performing policy. + +Generate the dataset with manipulation and point-to-point navigation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +To create a comprehensive locomanipulation dataset that combines both manipulation and navigation capabilities, you can generate a navigation dataset using the manipulation dataset from the previous step as input. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/disjoint_navigation.gif + :width: 100% + :align: center + :alt: G1 humanoid robot combining navigation with locomanipulation + :figclass: align-center + + G1 humanoid robot performing locomanipulation with navigation capabilities. + +The locomanipulation dataset generation process takes the previously generated manipulation dataset and creates scenarios where the robot must navigate from one location to another while performing manipulation tasks. This creates a more complex dataset that includes both locomotion and manipulation behaviors. + +To generate the locomanipulation dataset, use the following command: + +.. code:: bash + + ./isaaclab.sh -p \ + scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ + --device cpu \ + --kit_args="--enable isaacsim.replicator.mobility_gen" \ + --task="Isaac-G1-SteeringWheel-Locomanipulation" \ + --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 \ + --num_runs 1 \ + --lift_step 60 \ + --navigate_step 130 \ + --enable_pinocchio \ + --output_file ./datasets/generated_dataset_g1_locomanipulation_sdg.hdf5 \ + --enable_cameras + +.. note:: + + The input dataset (``--dataset``) should be the manipulation dataset generated in the previous step. You can specify any output filename using the ``--output_file_name`` parameter. + +The key parameters for locomanipulation dataset generation are: + +* ``--lift_step 70``: Number of steps for the lifting phase of the manipulation task. This should mark the point immediately after the robot has grasped the object. +* ``--navigate_step 120``: Number of steps for the navigation phase between locations. This should make the point where the robot has lifted the object and is ready to walk. +* ``--output_file``: Name of the output dataset file + +This process creates a dataset where the robot performs the manipulation task at different locations, requiring it to navigate between points while maintaining the learned manipulation behaviors. The resulting dataset can be used to train policies that combine both locomotion and manipulation capabilities. + +.. note:: + + You can visualize the robot trajectory results with the following script command: + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py --input_file datasets/generated_dataset_g1_locomanipulation_sdg.hdf5 --output_dir /PATH/TO/DESIRED_OUTPUT_DIR + +The data generated from this locomanipulation pipeline can also be used to finetune an imitation learning policy using GR00T N1.5. To do this, +you may convert the generated dataset to LeRobot format as expected by GR00T N1.5, and then run the finetuning script provided +in the GR00T N1.5 repository. An example closed-loop policy rollout is shown in the video below: + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation_sdg_disjoint_nav_groot_policy_4x.gif + :width: 100% + :align: center + :alt: Simulation rollout of GR00T N1.5 policy finetuned for locomanipulation + :figclass: align-center + + Simulation rollout of GR00T N1.5 policy finetuned for locomanipulation. + +The policy shown above uses the camera image, hand poses, hand joint positions, object pose, and base goal pose as inputs. +The output of the model is the target base velocity, hand poses, and hand joint positions for the next several timesteps. + + +Demo 3: Visuomotor Policy for a Humanoid Robot ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/gr-1_nut_pouring_policy.gif + :width: 100% + :align: center + :alt: GR-1 humanoid robot performing a pouring task + :figclass: align-center + Download the Dataset ^^^^^^^^^^^^^^^^^^^^ -Download the pre-generated dataset from `here `__ and place it under ``IsaacLab/datasets/generated_dataset_gr1_nut_pouring.hdf5``. -The dataset contains 1000 demonstrations of a humanoid robot performing a pouring/placing task that was +Download the pre-generated dataset from `here `__ and place it under ``IsaacLab/datasets/generated_dataset_gr1_nut_pouring.hdf5`` +(**Note: The dataset size is approximately 12GB**). The dataset contains 1000 demonstrations of a humanoid robot performing a pouring/placing task that was generated using Isaac Lab Mimic for the ``Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0`` task. .. hint:: @@ -526,7 +751,11 @@ generated using Isaac Lab Mimic for the ``Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic- Then, it drops the red beaker into the blue bin. Lastly, it places the yellow bowl onto the white scale. See the video in the :ref:`visualize-results-demo-2` section below for a visual demonstration of the task. - **Note that the following commands are only for your reference and are not required for this demo.** + **The success criteria for this task requires the red beaker to be placed in the blue bin, the green nut to be in the yellow bowl, + and the yellow bowl to be placed on top of the white scale.** + + .. attention:: + **The following commands are only for your reference and are not required for this demo.** To collect demonstrations: @@ -594,6 +823,10 @@ Record the normalization parameters for later use in the visualization step. .. note:: By default the trained models and logs will be saved to ``IsaacLab/logs/robomimic``. +You can also post-train a `GR00T `__ foundation model to deploy a Vision-Language-Action policy for the task. + +Please refer to the `IsaacLabEvalTasks `__ repository for more details. + .. _visualize-results-demo-2: Visualize the results @@ -626,6 +859,17 @@ Visualize the results of the trained policy by running the following command, us The trained visuomotor policy performing the pouring task in Isaac Lab. +.. note:: + + **Expected Success Rates and Timings for Visuomotor Nut Pour GR1T2 Task** + + * Success rate for data generation depends on the quality of human demonstrations (how well the user performs them) and dataset annotation quality. Both data generation and downstream policy success are sensitive to these factors and can show high variance. See :ref:`Common Pitfalls when Generating Data ` for tips to improve your dataset. + * Data generation for 1000 demonstrations takes approximately 10 hours on a RTX ADA 6000. + * Behavior Cloning (BC) policy success is typically 50-60% (evaluated on 50 rollouts) when trained on 1000 generated demonstrations for 600 epochs (default). Training takes approximately 15 hours on a RTX ADA 6000. + * Recommendation: Train for 600 epochs with 1000 generated demonstrations, and evaluate multiple checkpoints saved between the 300th and 600th epochs to select the best-performing policy. + +.. _common-pitfalls-generating-data: + Common Pitfalls when Generating Data ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst b/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst index c879e997740..9ffd47b401e 100644 --- a/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst +++ b/docs/source/overview/reinforcement-learning/rl_existing_scripts.rst @@ -8,6 +8,12 @@ from the environments into the respective libraries function argument and return RL-Games -------- +.. attention:: + + When using RL-Games with the Ray workflow for distributed training or hyperparameter tuning, + please be aware that due to security risks associated with Ray, this workflow is not intended + for use outside of a strictly controlled network environment. + - Training an agent with `RL-Games `__ on ``Isaac-Ant-v0``: @@ -175,16 +181,16 @@ SKRL Note that JAX GPU support is only available on Linux. JAX 0.6.0 or higher (built on CuDNN v9.8) is incompatible with Isaac Lab's PyTorch 2.7 (built on CuDNN v9.7), and therefore not supported. - To install a compatible version of JAX for CUDA 12 use ``pip install "jax[cuda12]<0.6.0"``, for example. + To install a compatible version of JAX for CUDA 12 use ``pip install "jax[cuda12]<0.6.0" "flax<0.10.7"``, for example. .. code:: bash # install python module (for skrl) ./isaaclab.sh -i skrl + # install jax<0.6.0 for torch 2.7 + ./isaaclab.sh -p -m pip install "jax[cuda12]<0.6.0" "flax<0.10.7" # install skrl dependencies for JAX ./isaaclab.sh -p -m pip install skrl["jax"] - # install jax<0.6.0 for torch 2.7 - ./isaaclab.sh -p -m pip install "jax[cuda12]<0.6.0" # run script for training ./isaaclab.sh -p scripts/reinforcement_learning/skrl/train.py --task Isaac-Reach-Franka-v0 --headless --ml_framework jax # run script for playing with 32 environments diff --git a/docs/source/refs/contributing.rst b/docs/source/refs/contributing.rst index f4bc45003b7..85721ff0f49 100644 --- a/docs/source/refs/contributing.rst +++ b/docs/source/refs/contributing.rst @@ -112,11 +112,8 @@ integrated with the `NVIDIA Omniverse Platform `__. -To use this content, you can use the Asset Browser provided in Isaac Sim. - -Please check the `Isaac Sim documentation `__ -for more information on how to download the assets. +Please checkout the `Isaac Sim Assets `__ +for more information on what is presently available. .. attention:: diff --git a/docs/source/refs/issues.rst b/docs/source/refs/issues.rst index da9efa40dcd..c4bb56182e5 100644 --- a/docs/source/refs/issues.rst +++ b/docs/source/refs/issues.rst @@ -93,6 +93,17 @@ message and continue with terminating the process. On Windows systems, please us ``Ctrl+Break`` or ``Ctrl+fn+B`` to terminate the process. +URDF Importer: Unresolved references for fixed joints +----------------------------------------------------- + +Starting with Isaac Sim 5.1, links connected through ``fixed_joint`` elements are no longer merged when +their URDF link entries specify mass and inertia even if ``merge-joint`` set to True. +This is expected behaviour—those links are treated as full bodies rather than zero-mass reference frames. +However, the USD importer currently raises ``ReportError`` warnings showing unresolved references for such links +when they lack visuals or colliders. This is a known bug in the importer; it creates references to visuals +that do not exist. The warnings can be safely ignored until the importer is updated. + + GLIBCXX errors in Conda ----------------------- diff --git a/docs/source/refs/reference_architecture/index.rst b/docs/source/refs/reference_architecture/index.rst index 34296144506..c875c964e26 100644 --- a/docs/source/refs/reference_architecture/index.rst +++ b/docs/source/refs/reference_architecture/index.rst @@ -1,3 +1,5 @@ +.. _ref_arch: + Reference Architecture ====================== @@ -195,10 +197,12 @@ Some wrappers include: * `Video Wrappers `__ * `RL Libraries Wrappers `__ +.. currentmodule:: isaaclab_rl + Most RL libraries expect their own variation of an environment interface. This means the data types needed by each library differs. Isaac Lab provides its own wrappers to convert the environment into the expected interface by the RL library a user wants to use. These are -specified in the `Isaac Lab utils wrapper module `__. +specified in :class:`isaaclab_rl` See the `full list `__ of other wrappers APIs. For more information on how these wrappers work, please refer to the `Wrapping environments `__ documentation. @@ -345,7 +349,7 @@ Check out our resources on using Isaac Lab with your robots. Review Our Documentation & Samples Resources -* `Isaac Lab Tutorials`_ +* :ref:`Isaac Lab Tutorials ` * `Fast-Track Robot Learning in Simulation Using NVIDIA Isaac Lab`_ * `Supercharge Robotics Workflows with AI and Simulation Using NVIDIA Isaac Sim 4.0 and NVIDIA Isaac Lab`_ * `Closing the Sim-to-Real Gap: Training Spot Quadruped Locomotion with NVIDIA Isaac Lab `__ @@ -360,16 +364,15 @@ Learn More About Featured NVIDIA Solutions .. _curriculum learning: https://arxiv.org/abs/2109.11978 .. _CAD Converter: https://docs.omniverse.nvidia.com/extensions/latest/ext_cad-converter.html -.. _URDF Importer: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_urdf.html -.. _MJCF Importer: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_mjcf.html +.. _URDF Importer: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_urdf.html +.. _MJCF Importer: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_mjcf.html .. _Onshape Importer: https://docs.omniverse.nvidia.com/extensions/latest/ext_onshape.html -.. _Isaac Sim Reference Architecture: https://docs.isaacsim.omniverse.nvidia.com/latest/isaac_sim_reference_architecture.html -.. _Importing Assets section: https://docs.isaacsim.omniverse.nvidia.com/latest/isaac_sim_reference_architecture.html#importing-assets +.. _Isaac Sim Reference Architecture: https://docs.isaacsim.omniverse.nvidia.com/latest/introduction/reference_architecture.html +.. _Importing Assets section: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/importers_exporters.html .. _Scale AI-Enabled Robotics Development Workloads with NVIDIA OSMO: https://developer.nvidia.com/blog/scale-ai-enabled-robotics-development-workloads-with-nvidia-osmo/ .. _Isaac Perceptor: https://developer.nvidia.com/isaac/perceptor .. _Isaac Manipulator: https://developer.nvidia.com/isaac/manipulator .. _Additional Resources: https://isaac-sim.github.io/IsaacLab/main/source/refs/additional_resources.html -.. _Isaac Lab Tutorials: file:///home/oomotuyi/isaac/IsaacLab/docs/_build/current/source/tutorials/index.html .. _Fast-Track Robot Learning in Simulation Using NVIDIA Isaac Lab: https://developer.nvidia.com/blog/fast-track-robot-learning-in-simulation-using-nvidia-isaac-lab/ .. _Supercharge Robotics Workflows with AI and Simulation Using NVIDIA Isaac Sim 4.0 and NVIDIA Isaac Lab: https://developer.nvidia.com/blog/supercharge-robotics-workflows-with-ai-and-simulation-using-nvidia-isaac-sim-4-0-and-nvidia-isaac-lab/ diff --git a/docs/source/refs/release_notes.rst b/docs/source/refs/release_notes.rst index be9dc4d8ec1..57d5e1891cc 100644 --- a/docs/source/refs/release_notes.rst +++ b/docs/source/refs/release_notes.rst @@ -4,6 +4,243 @@ Release Notes The release notes are now available in the `Isaac Lab GitHub repository `_. We summarize the release notes here for convenience. +v2.3.0 +====== + +What's Changed +-------------- + +The Isaac Lab 2.3.0 release, built on Isaac Sim 5.1, delivers enhancements across dexterous manipulation, +teleoperation, and learning workflows. It introduces new dexterous environments with advanced training capabilities, +expands surface gripper and teleoperation support for a wider range of robots and devices, +and integrates SkillGen with the Mimic imitation learning pipeline to enable GPU-accelerated motion planning +and skill-based data generation with cuRobo integration. + +Key highlights of this release include: + +* **Dexterous RL (DexSuite)**: Introduction of two new dexterous manipulation environments using the Kuka arm and + Allegro hand setup, with addition of support for Automatic Domain Randomization (ADR) and PBT (Population-Based Training). +* **Surface gripper updates**: Surface gripper has been extended to support Manager-based workflows, + including the addition of ``SurfaceGripperAction`` and ``SurfaceGripperActionCfg``, along with several new environments + demonstrating teleoperation examples with surface grippers and the RMPFlow controller. + New robots and variations are introduced, including Franka and UR10 with robotiq grippers and suction cups, + and Galbot and Agibot robots. +* **Mimic - SkillGen**: SkillGen support has been added for the Mimic Imitation Learning pipeline, + introducing cuRobo integration, integrating GPU motion planning with skill-segmented data generation. + Note that cuRobo has proprietary licensing terms, please review the + `cuRobo license `_ + carefully before use. +* **Mimic - Locomanipulation**: Added a new G1 humanoid environment combining RL-based locomotion with IK-based + manipulation. A full robot navigation stack is integrated to augment demonstrations with randomization of + tabletop pick/place locations, destination and ground obstacles. By segmenting tasks into pick-navigate-place + phases, this method enables generation of large-scale loco-manipulation datasets from manipulation-only + demonstrations. +* **Teleoperation**: Upper body inverse kinematics controller is improved by adding a null space posture task that + helps enable waist movement on humanoid tasks while regularizing redundant degrees-of-freedom to a preferred + upright posture. Additionally, support for Vive and Manus Glove are introduced, providing more options for + teleoperation devices. + +**Full Changelog**: https://github.com/isaac-sim/IsaacLab/compare/v2.2.1...v2.3.0 + +Isaac Sim 5.1 Updates +---------------------- + +* Introduced support for `DGX Spark `_, + including multi-architecture Docker images with support for ARM platforms. +* PhysX now offers a new joint parameter tuning `tutorial `_ + for robotic grippers, along with a new feature for solving articulation collision contacts last to improve on + gripper penetration issues, especially for cases with sub-optimally tuned joints. +* Surface grippers has been optimized for better performance. Although support continues to be CPU-only, + performance has improved by several orders of magnitude compared to previous releases. +* Windows 10 support ended on October 14, 2025. Microsoft will no longer provide free security, feature, or technical + updates for Windows 10. As a result, we will be dropping support for Windows 10 in future releases of Isaac Sim and Lab + to ensure the security and functionality of our software. + +New Features +------------ + +Core +~~~~ + +* Supports rl games wrapper with dictionary observation by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3340 +* Adds surface gripper support in manager-based workflow by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3174 +* Adds two new robots with grippers by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3229 +* Adds new Collision Mesh Schema properties by @hapatel-bdai in https://github.com/isaac-sim/IsaacLab/pull/2249 +* Adds PBT algorithm to rl games by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3399 + +Mimic and Teleoperation +~~~~~~~~~~~~~~~~~~~~~~~ + +* Adds SkillGen framework to Isaac Lab with cuRobo support by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3303 +* Adds locomanipulation data generation via. disjoint navigation by @jaybdub in https://github.com/isaac-sim/IsaacLab/pull/3259 +* Adds support for manus and vive by @cathyliyuanchen in https://github.com/isaac-sim/IsaacLab/pull/3357 +* Adds notification widgets at IK error status and Teleop task completion by @lotusl-code in https://github.com/isaac-sim/IsaacLab/pull/3356 + +Environments +~~~~~~~~~~~~ + +* Adds dexterous lift and reorientation manipulation environments by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3378 +* Adds task Reach-UR10e, an end-effector tracking environment by @ashwinvkNV in https://github.com/isaac-sim/IsaacLab/pull/3147 +* Adds a configuration example for Student-Teacher Distillation by @ClemensSchwarke in https://github.com/isaac-sim/IsaacLab/pull/3100 +* Adds Locomanipulation Environment with G1 for Mimic workflow by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3150 +* Adds teleop support for Unitree G1 with Inspire 5-finger hand, take PickPlace task as an example by @yami007007 in https://github.com/isaac-sim/IsaacLab/pull/3242 +* Adds galbot stack cube tasks, with left_arm_gripper and right_arm_suction, using RMPFlow controller by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3210 +* Adds AVP teleop support for Galbot stack tasks by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3669 +* Adds camera to G1 Steering Wheel environment by @jaybdub in https://github.com/isaac-sim/IsaacLab/pull/3549 + +Infrastructure +~~~~~~~~~~~~~~ + +* Adds YAML Resource Specification To Ray Integration by @binw666 in https://github.com/isaac-sim/IsaacLab/pull/2847 +* Installs cuda13 on arm builds for Spark by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3396 +* Adds arm64 platform for Pink IK setup by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3686 +* Updates torch installation version to 2.9 for Linux-aarch, and updates opset version from 11 to 18. by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3706 + + +Improvements +------------ + +Core and Infrastructure +~~~~~~~~~~~~~~~~~~~~~~~ + +* Adds changes for rsl_rl 3.0.1 by @ClemensSchwarke in https://github.com/isaac-sim/IsaacLab/pull/2962 +* Simplifies cross platform installation setup.py by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3294 +* Updated image build logic and details by @nv-apoddubny in https://github.com/isaac-sim/IsaacLab/pull/3649 +* Applies the pre-merge CI failure control to the tasks by @nv-apoddubny in https://github.com/isaac-sim/IsaacLab/pull/3457 +* Updates Isaac Sim 5.1 staging server to production by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3691 +* Removes scikit-learn dependency by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3799 +* Removes extra calls to write simulation after reset_idx by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3446 +* Exposes render parameter ``/rtx/domeLight/upperLowerStrategy`` for dome light by @shauryadNv in https://github.com/isaac-sim/IsaacLab/pull/3694 +* Adds onnxscript dependency to isaaclab_rl module by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3722 +* Configures mesh collision schemas in ``convert_mesh.py`` by @zehao-wang in https://github.com/isaac-sim/IsaacLab/pull/3558 + +Mimic and Teleoperation +~~~~~~~~~~~~~~~~~~~~~~~ + +* Improves recorder performance and add additional recording capability by @peterd-NV in https://github.com/isaac-sim/IsaacLab/pull/3302 +* Optimizes Kit XR Teleop CPU time by @hougantc-nvda in https://github.com/isaac-sim/IsaacLab/pull/3487 +* Improves dataset file names and low success rate for trained model on g1 locomanipulation dataset by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3503 +* Updates the teleop_se3 and record_demos scripts with more helpful description for teleop_device parameter by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3642 + + +Documentation +------------- + +Core +~~~~ + +* Updates documentation to explain known issue of missing references when uses URDF importer by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3729 +* Fixes symbol in training_jetbot_reward_exploration.rst by @dougfulop in https://github.com/isaac-sim/IsaacLab/pull/2722 +* Clarifies asset classes' default_inertia tensor coordinate frame by @preist-nvidia in https://github.com/isaac-sim/IsaacLab/pull/3405 +* Adds limitation note in docs for Multi Node Training on DGX Spark by @matthewtrepte in https://github.com/isaac-sim/IsaacLab/pull/3806 +* Updates locomanip task name and link in docs by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/3342 + +Mimic and Teleoperation +~~~~~~~~~~~~~~~~~~~~~~~ + +* Fixes G1 dataset link in teleop_imitation tutorial by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3463 +* Updates dataset instruction in ``teleop_imitation.rst`` (#3462) by @peterd-NV in https://github.com/isaac-sim/IsaacLab/pull/3489 +* Fixes teleop doc in Isaac Lab by @tifchen-nvda in https://github.com/isaac-sim/IsaacLab/pull/3539 +* Updates cloudxr teleop doc in Isaac Lab by @tifchen-nvda in https://github.com/isaac-sim/IsaacLab/pull/3540 +* Adds instructions on how to position the lighthouse for manus+vive by @cathyliyuanchen in https://github.com/isaac-sim/IsaacLab/pull/3548 +* Corrects versions for the cloudxr teleop doc by @tifchen-nvda in https://github.com/isaac-sim/IsaacLab/pull/3580 +* Adds link to IsaacLabEvalTasks repo from mimic section in doc (#3621) by @xyao-nv in https://github.com/isaac-sim/IsaacLab/pull/3627 +* Fixes ordering of docs for imitation learning by @shauryadNv in https://github.com/isaac-sim/IsaacLab/pull/3634 +* Updates documentation for manus teleop by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3605 +* Updates SkillGen documentation for data gen command and success rates by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3703 +* Fixes typo in mimic teleop documentation for locomanipulation by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3704 +* Updates dataset paths in teleop documentation and adds note in documentation to adjusting AR Anchors by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3707 +* Adds pysurvive installation instructions by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3747 +* Adds to mimic documentation expected generation and training timings and success rates by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3742 +* Adds data gen and policy learning times in SkillGen documentation by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3774 +* Updates doc to describe ways to clean up orphaned container and check connectivity for teleop by @yanziz-nvidia in https://github.com/isaac-sim/IsaacLab/pull/3787 +* Updates cloudxr teleop doc to explain openxr plugin by @tifchen-nvda in https://github.com/isaac-sim/IsaacLab/pull/3786 +* Updates Mimic docs to clarify CPU mode usage and DGX Spark support by @peterd-NV in https://github.com/isaac-sim/IsaacLab/pull/3794 +* Updates cuRobo installation instructions and added VRAM baseline perf to SkillGen docs by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3797 +* Adds dgx spark limitations link to teleop docs by @lotusl-code in https://github.com/isaac-sim/IsaacLab/pull/3805 +* Adds Cosmos Transfer1 limitation for DGX spark by @shauryadNv in https://github.com/isaac-sim/IsaacLab/pull/3817 +* Updates DGX spark limitations for SkillGen in the documentation by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3748 +* Adds the Isaac-PickPlace-G1-InspireFTP-Abs-v0 Task into Envs Docs by @yami007007 in https://github.com/isaac-sim/IsaacLab/pull/3479 + +Infrastructure +~~~~~~~~~~~~~~ + +* Change GLIBC version requirement to 2.35 for pip by @GiulioRomualdi in https://github.com/isaac-sim/IsaacLab/pull/3360 +* Updates Isaac Sim license by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3393 +* Updates jax installation instructions by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3561 +* Adds section for the DGX spark limitations by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/3652 +* Fixes broken links in the documentation by @mpgussert in https://github.com/isaac-sim/IsaacLab/pull/3721 +* Adds windows pip installation instruction in local pip installation documentation by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3723 +* Adds note about potential security risks with Ray by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3711 +* Fixes errors while building the docs by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3370 + + +Bug Fixes +--------- + +Core +~~~~ + +* Fixes missing visible attribute in spawn_ground_plane by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3304 +* Moves parameter ``platform_height`` to the correct mesh terrain configuration by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3316 +* Fixes invalid callbacks for debug vis when simulation is restarted by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3338 +* Deletes unused asset.py in isaaclab by @fan-ziqi in https://github.com/isaac-sim/IsaacLab/pull/3389 +* Moves location of serve file check to the correct module by @Mayankm96 in https://github.com/isaac-sim/IsaacLab/pull/3368 +* Fixes SurfaceGripper API to accommodate for Isaac Sim 5.1 changes by @AntoineRichard in https://github.com/isaac-sim/IsaacLab/pull/3528 +* Fixes keyboard unsubscribe carb call by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3662 +* Fixes GCC error for raycaster demo when running in conda by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3712 +* Corrects materials and objects imports in ``check_terrain_importer.py`` by @PeterL-NV in https://github.com/isaac-sim/IsaacLab/pull/3411 +* Fixes tensor construction warning in ``events.py`` by @louislelay in https://github.com/isaac-sim/IsaacLab/pull/3251 +* Fixes skrl train/play script configurations when using the ``--agent`` argument and rename agent configuration variable by @Toni-SM in https://github.com/isaac-sim/IsaacLab/pull/3643 +* Fixes TiledCamera data types and rlgames training on CPU by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3808 + +Mimic and Teleoperation +~~~~~~~~~~~~~~~~~~~~~~~ + +* Updates the Path to Isaaclab Dir in SkillGen Documentation by @njawale42 in https://github.com/isaac-sim/IsaacLab/pull/3483 +* Fixes the reach task regression with teleop devices returning the gripper by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3327 +* Fixes teleop G1 with Inspire hand issues by @yami007007 in https://github.com/isaac-sim/IsaacLab/pull/3440 +* Updates default viewer pose to see the whole scene for Agibot environment by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3525 +* Fixes XR UI when used with teleop devices other than "handtracking" by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3566 +* Fixes manus joint indices mapping for teleoperation by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3592 +* Updates gr1t2 dex pilot hand scaling by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3607 +* Fixes unreal surface_gripper behavior by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3679 +* Fixes G1 finger PD gains configs for locomanipulation by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3749 +* Fixes the bug of right_arm suction cup passing through cubes by @rebeccazhang0707 in https://github.com/isaac-sim/IsaacLab/pull/3764 +* Updates the xr anchor for g1 tasks to me more natural for standing teleop by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3775 +* Suppresses dex_retargeting::yourdfpy warnings for G1 by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3798 +* Refines height of xr view for G1 envs by @rwiltz in https://github.com/isaac-sim/IsaacLab/pull/3813 + +Infrastructure +~~~~~~~~~~~~~~ + +* Fixes the missing Ray initialization by @ozhanozen in https://github.com/isaac-sim/IsaacLab/pull/3350 +* Fixes torch nightly version install in arm system by @ooctipus in https://github.com/isaac-sim/IsaacLab/pull/3464 +* Fixes unintentional removal of '=' from command by @ndahile-nvidia in https://github.com/isaac-sim/IsaacLab/pull/3600 +* Updates installation script for aarch64 to fix LD_PRELOAD issues by @matthewtrepte in https://github.com/isaac-sim/IsaacLab/pull/3708 +* Fixes hanging issue in test_manager_based_rl_env_obs_spaces.py by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3717 +* Fixes for missing desktop icon when running scripts on DGX Spark by @matthewtrepte in https://github.com/isaac-sim/IsaacLab/pull/3804 + + +Breaking Changes +---------------- + +* Removes unused 'relevant_link_name' parameter in nutpour and exhaust pipe envs by @michaellin6 in https://github.com/isaac-sim/IsaacLab/pull/3651 +* Moves IO descriptor log dir to logs by @kellyguo11 in https://github.com/isaac-sim/IsaacLab/pull/3434 + +Known Issues +~~~~~~~~~~~~ + +* The ROS2 docker image is not currently expected to work due to the update to Python 3.11. We are actively working on + a fix to resolve this. +* We have received reports of performance regressions in the previous Isaac Sim release for both physics and rendering + workflows. We are still working on addressing some of these, but have also found some workarounds. + For viewport regressions, Omniverse settings can be set by adding + ``--kit_args="--/app/usdrt/hierarchy/partialGpuUpdate=1 --/rtx/post/dlss/execMode=0 --/app/runLoops/main/rateLimitEnabled=false --/app/runLoops/main/manualModeEnabled=true --enable omni.kit.loop-isaac"``. Additionally, Isaac Sim 5.0 + introduced new actuator models for PhysX, including drive model and friction model improvements. + These improvements also introduced a small performance regression. We have observed up to ~20% slowdown in some + state-based environments. + v2.2.1 ====== @@ -1348,7 +1585,7 @@ Welcome to the first official release of Isaac Lab! Building upon the foundation of the `Orbit `_ framework, we have integrated the RL environment designing workflow from `OmniIsaacGymEnvs `_. -This allows users to choose a suitable `task-design approach `_ +This allows users to choose a suitable :ref:`task-design approach ` for their applications. While we maintain backward compatibility with Isaac Sim 2023.1.1, we highly recommend using Isaac Lab with @@ -1361,12 +1598,12 @@ New Features * Integrated CI/CD pipeline, which is triggered on pull requests and publishes the results publicly * Extended support for Windows OS platforms -* Added `tiled rendered `_ based Camera +* Added tiled render based Camera sensor implementation. This provides optimized RGB-D rendering throughputs of up to 10k frames per second. * Added support for multi-GPU and multi-node training for the RL-Games library * Integrated APIs for environment designing (direct workflow) without relying on managers * Added implementation of delayed PD actuator model -* `Added various new learning environments `_: +* Added various new learning environments: * Cartpole balancing using images * Shadow hand cube reorientation * Boston Dynamics Spot locomotion diff --git a/docs/source/refs/troubleshooting.rst b/docs/source/refs/troubleshooting.rst index ea6ac86882c..18a88da7c69 100644 --- a/docs/source/refs/troubleshooting.rst +++ b/docs/source/refs/troubleshooting.rst @@ -78,32 +78,6 @@ For instance, to run a standalone script with verbose logging, you can use the f For more fine-grained control, you can modify the logging channels through the ``omni.log`` module. For more information, please refer to its `documentation `__. -Using CPU Scaling Governor for performance ------------------------------------------- - -By default on many systems, the CPU frequency governor is set to -“powersave” mode, which sets the CPU to lowest static frequency. To -increase the maximum performance, we recommend setting the CPU frequency -governor to “performance” mode. For more details, please check the the -link -`here `__. - -.. warning:: - We advice not to set the governor to “performance” mode on a system with poor - cooling (such as laptops), since it may cause the system to overheat. - -- To view existing ``scaling_governor`` value per CPU: - - .. code:: bash - - cat /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor - -- To change the governor to “performance” mode for each CPU: - - .. code:: bash - - echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor - Observing long load times at the start of the simulation -------------------------------------------------------- diff --git a/docs/source/setup/installation/asset_caching.rst b/docs/source/setup/installation/asset_caching.rst index 5ee0760b681..5cee207fae3 100644 --- a/docs/source/setup/installation/asset_caching.rst +++ b/docs/source/setup/installation/asset_caching.rst @@ -8,7 +8,7 @@ In some cases, it is possible that asset loading times can be long when assets a If you run into cases where assets take a few minutes to load for each run, we recommend enabling asset caching following the below steps. -First, launch the Isaac Sim app: +First, launch the Isaac Sim application: .. tab-set:: :sync-group: os @@ -27,25 +27,32 @@ First, launch the Isaac Sim app: isaaclab.bat -s -On the top right of the Isaac Sim app, there will be an icon labelled ``CACHE:``. -There may be a message indicating ``HUB NOT DETECTED`` or ``NEW VERSION DETECTED``. +On the top right of the Isaac Lab or Isaac Sim app, look for the icon labeled ``CACHE:``. +You may see a message such as ``HUB NOT DETECTED`` or ``NEW VERSION DETECTED``. -.. figure:: ../../_static/setup/asset_caching.jpg +Click the message to enable `Hub `_. +Hub automatically manages local caching for Isaac Lab assets, so subsequent runs will use cached files instead of +downloading from AWS each time. + +.. figure:: /source/_static/setup/asset_caching.jpg :align: center :figwidth: 100% :alt: Simulator with cache messaging. -Click on the message, which will enable `Hub `_ -for asset caching. Once enabled, Hub will run automatically each time an Isaac Lab or Isaac Sim instance is run. +Hub provides better control and management of cached assets, making workflows faster and more reliable, especially +in environments with limited or intermittent internet access. -Note that for the first run, assets will still need to be pulled from the cloud, which could lead to longer loading times. -However, subsequent runs that use the same assets will be able to use the cached files from Hub. -Hub will provide better control for caching of assets used in Isaac Lab. +.. note:: + The first time you run Isaac Lab, assets will still need to be pulled from the cloud, which could lead + to longer loading times. Once cached, loading times will be significantly reduced on subsequent runs. Nucleus ------- -Prior to Isaac Sim 4.5, assets were accessible from the Omniverse Nucleus server and through setting up a local Nucleus server. -Although from Isaac Sim 4.5, we have deprecated the use of Omniverse Nucleus and the Omniverse Launcher, any existing instances -or setups of local Nucleus instances should still work. We recommend keeping existing setups if a local Nucleus server -was previously already set up. + +Before Isaac Sim 4.5, assets were accessed via the Omniverse Nucleus server, including setups with local Nucleus instances. + +.. warning:: + Starting with Isaac Sim 4.5, the Omniverse Nucleus server and Omniverse Launcher are deprecated. + Existing Nucleus setups will continue to work, so if you have a local Nucleus server already configured, + you may continue to use it. diff --git a/docs/source/setup/installation/binaries_installation.rst b/docs/source/setup/installation/binaries_installation.rst index d066f7ce0b0..82754d6871e 100644 --- a/docs/source/setup/installation/binaries_installation.rst +++ b/docs/source/setup/installation/binaries_installation.rst @@ -1,9 +1,9 @@ .. _isaaclab-binaries-installation: -Installation using Isaac Sim Binaries -===================================== +Installation using Isaac Sim Pre-built Binaries +=============================================== -Isaac Lab requires Isaac Sim. This tutorial installs Isaac Sim first from binaries, then Isaac Lab from source code. +The following steps first installs Isaac Sim from its pre-built binaries, then Isaac Lab from source code. Installing Isaac Sim -------------------- @@ -11,14 +11,14 @@ Installing Isaac Sim Downloading pre-built binaries ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Please follow the Isaac Sim -`documentation `__ -to install the latest Isaac Sim release. +Isaac Sim binaries can be downloaded directly as a zip file from +`here `__. +If you wish to use the older Isaac Sim 4.5 release, please check the older download page +`here `__. -From Isaac Sim 4.5 release, Isaac Sim binaries can be `downloaded `_ directly as a zip file. - -To check the minimum system requirements, refer to the documentation -`here `__. +Once the zip file is downloaded, you can unzip it to the desired directory. +As an example set of instructions for unzipping the Isaac Sim binaries, +please refer to the `Isaac Sim documentation `__. .. tab-set:: :sync-group: os @@ -26,23 +26,12 @@ To check the minimum system requirements, refer to the documentation .. tab-item:: :icon:`fa-brands fa-linux` Linux :sync: linux - .. note:: - - For details on driver requirements, please see the `Technical Requirements `_ guide! - - On Linux systems, Isaac Sim directory will be named ``${HOME}/isaacsim``. + On Linux systems, we assume the Isaac Sim directory is named ``${HOME}/isaacsim``. .. tab-item:: :icon:`fa-brands fa-windows` Windows :sync: windows - .. note:: - - For details on driver requirements, please see the `Technical Requirements `_ guide! - - From Isaac Sim 4.5 release, Isaac Sim binaries can be downloaded directly as a zip file. - The below steps assume the Isaac Sim folder was unzipped to the ``C:/isaacsim`` directory. - - On Windows systems, Isaac Sim directory will be named ``C:/isaacsim``. + On Windows systems, we assume the Isaac Sim directory is named ``C:\isaacsim``. Verifying the Isaac Sim installation ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -70,412 +59,22 @@ variables to your terminal for the remaining of the installation instructions: .. code:: batch :: Isaac Sim root directory - set ISAACSIM_PATH="C:/isaacsim" + set ISAACSIM_PATH="C:\isaacsim" :: Isaac Sim python executable set ISAACSIM_PYTHON_EXE="%ISAACSIM_PATH:"=%\python.bat" -For more information on common paths, please check the Isaac Sim -`documentation `__. - - -- Check that the simulator runs as expected: - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # note: you can pass the argument "--help" to see all arguments possible. - ${ISAACSIM_PATH}/isaac-sim.sh - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: note: you can pass the argument "--help" to see all arguments possible. - %ISAACSIM_PATH%\isaac-sim.bat - - -- Check that the simulator runs from a standalone python script: - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # checks that python path is set correctly - ${ISAACSIM_PYTHON_EXE} -c "print('Isaac Sim configuration is now complete.')" - # checks that Isaac Sim can be launched from python - ${ISAACSIM_PYTHON_EXE} ${ISAACSIM_PATH}/standalone_examples/api/isaacsim.core.api/add_cubes.py - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: checks that python path is set correctly - %ISAACSIM_PYTHON_EXE% -c "print('Isaac Sim configuration is now complete.')" - :: checks that Isaac Sim can be launched from python - %ISAACSIM_PYTHON_EXE% %ISAACSIM_PATH%\standalone_examples\api\isaacsim.core.api\add_cubes.py - - -.. caution:: - - If you have been using a previous version of Isaac Sim, you need to run the following command for the *first* - time after installation to remove all the old user data and cached variables: - - .. tab-set:: - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - - .. code:: bash - - ${ISAACSIM_PATH}/isaac-sim.sh --reset-user - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - - .. code:: batch - - %ISAACSIM_PATH%\isaac-sim.bat --reset-user - - -If the simulator does not run or crashes while following the above -instructions, it means that something is incorrectly configured. To -debug and troubleshoot, please check Isaac Sim -`documentation `__ -and the -`forums `__. - +.. include:: include/bin_verify_isaacsim.rst Installing Isaac Lab -------------------- -Cloning Isaac Lab -~~~~~~~~~~~~~~~~~ - -.. note:: - - We recommend making a `fork `_ of the Isaac Lab repository to contribute - to the project but this is not mandatory to use the framework. If you - make a fork, please replace ``isaac-sim`` with your username - in the following instructions. - -Clone the Isaac Lab repository into your workspace: - -.. tab-set:: - - .. tab-item:: SSH - - .. code:: bash - - git clone git@github.com:isaac-sim/IsaacLab.git - - .. tab-item:: HTTPS - - .. code:: bash - - git clone https://github.com/isaac-sim/IsaacLab.git +.. include:: include/src_clone_isaaclab.rst +.. include:: include/src_symlink_isaacsim.rst -.. note:: - We provide a helper executable `isaaclab.sh `_ that provides - utilities to manage extensions: - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: text - - ./isaaclab.sh --help - - usage: isaaclab.sh [-h] [-i] [-f] [-p] [-s] [-t] [-o] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. - - optional arguments: - -h, --help Display the help content. - -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl-games, rsl-rl, sb3, skrl) as extra dependencies. Default is 'all'. - -f, --format Run pre-commit to format the code and check lints. - -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). - -s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim. - -t, --test Run all python pytest tests. - -o, --docker Run the docker container helper script (docker/container.sh). - -v, --vscode Generate the VSCode settings file from template. - -d, --docs Build the documentation from source using sphinx. - -n, --new Create a new external project or internal task from template. - -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: text - - isaaclab.bat --help - - usage: isaaclab.bat [-h] [-i] [-f] [-p] [-s] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. - - optional arguments: - -h, --help Display the help content. - -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl-games, rsl-rl, sb3, skrl) as extra dependencies. Default is 'all'. - -f, --format Run pre-commit to format the code and check lints. - -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). - -s, --sim Run the simulator executable (isaac-sim.bat) provided by Isaac Sim. - -t, --test Run all python pytest tests. - -v, --vscode Generate the VSCode settings file from template. - -d, --docs Build the documentation from source using sphinx. - -n, --new Create a new external project or internal task from template. - -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. - - -Creating the Isaac Sim Symbolic Link -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -Set up a symbolic link between the installed Isaac Sim root folder -and ``_isaac_sim`` in the Isaac Lab directory. This makes it convenient -to index the python modules and look for extensions shipped with Isaac Sim. - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # enter the cloned repository - cd IsaacLab - # create a symbolic link - ln -s path_to_isaac_sim _isaac_sim - # For example: ln -s ${HOME}/isaacsim _isaac_sim - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: enter the cloned repository - cd IsaacLab - :: create a symbolic link - requires launching Command Prompt with Administrator access - mklink /D _isaac_sim path_to_isaac_sim - :: For example: mklink /D _isaac_sim C:/isaacsim - - -Setting up the conda environment (optional) -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. attention:: - This step is optional. If you are using the bundled python with Isaac Sim, you can skip this step. - -.. note:: - - If you use Conda, we recommend using `Miniconda `_. - -The executable ``isaaclab.sh`` automatically fetches the python bundled with Isaac -Sim, using ``./isaaclab.sh -p`` command (unless inside a virtual environment). This executable -behaves like a python executable, and can be used to run any python script or -module with the simulator. For more information, please refer to the -`documentation `__. - -To install ``conda``, please follow the instructions `here `__. -You can create the Isaac Lab environment using the following commands. - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # Option 1: Default name for conda environment is 'env_isaaclab' - ./isaaclab.sh --conda # or "./isaaclab.sh -c" - # Option 2: Custom name for conda environment - ./isaaclab.sh --conda my_env # or "./isaaclab.sh -c my_env" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: Option 1: Default name for conda environment is 'env_isaaclab' - isaaclab.bat --conda :: or "isaaclab.bat -c" - :: Option 2: Custom name for conda environment - isaaclab.bat --conda my_env :: or "isaaclab.bat -c my_env" - - -Once created, be sure to activate the environment before proceeding! - -.. code:: bash - - conda activate env_isaaclab # or "conda activate my_env" - -Once you are in the virtual environment, you do not need to use ``./isaaclab.sh -p`` / ``isaaclab.bat -p`` -to run python scripts. You can use the default python executable in your environment -by running ``python`` or ``python3``. However, for the rest of the documentation, -we will assume that you are using ``./isaaclab.sh -p`` / ``isaaclab.bat -p`` to run python scripts. This command -is equivalent to running ``python`` or ``python3`` in your virtual environment. - - -Installation -~~~~~~~~~~~~ - -- Install dependencies using ``apt`` (on Linux only): - - .. code:: bash - - # these dependency are needed by robomimic which is not available on Windows - sudo apt install cmake build-essential - -- Run the install command that iterates over all the extensions in ``source`` directory and installs them - using pip (with ``--editable`` flag): - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh --install # or "./isaaclab.sh -i" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat --install :: or "isaaclab.bat -i" - -.. note:: - - By default, the above will install all the learning frameworks. If you want to install only a specific framework, you can - pass the name of the framework as an argument. For example, to install only the ``rl_games`` framework, you can run - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh --install rl_games # or "./isaaclab.sh -i rl_games" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat --install rl_games :: or "isaaclab.bat -i rl_games" - - The valid options are ``rl_games``, ``rsl_rl``, ``sb3``, ``skrl``, ``robomimic``, ``none``. - - -Verifying the Isaac Lab installation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -To verify that the installation was successful, run the following command from the -top of the repository: - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # Option 1: Using the isaaclab.sh executable - # note: this works for both the bundled python and the virtual environment - ./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py - - # Option 2: Using python in your virtual environment - python scripts/tutorials/00_sim/create_empty.py - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: Option 1: Using the isaaclab.bat executable - :: note: this works for both the bundled python and the virtual environment - isaaclab.bat -p scripts\tutorials\00_sim\create_empty.py - - :: Option 2: Using python in your virtual environment - python scripts\tutorials\00_sim\create_empty.py - - -The above command should launch the simulator and display a window with a black -viewport. You can exit the script by pressing ``Ctrl+C`` on your terminal. -On Windows machines, please terminate the process from Command Prompt using -``Ctrl+Break`` or ``Ctrl+fn+B``. - -.. figure:: ../../_static/setup/verify_install.jpg - :align: center - :figwidth: 100% - :alt: Simulator with a black window. - - -If you see this, then the installation was successful! |:tada:| - -If you see an error ``ModuleNotFoundError: No module named 'isaacsim'``, ensure that the conda environment is activated -and ``source _isaac_sim/setup_conda_env.sh`` has been executed. - - -Train a robot! -~~~~~~~~~~~~~~~ - -You can now use Isaac Lab to train a robot through Reinforcement Learning! The quickest way to use Isaac Lab is through the predefined workflows using one of our **Batteries-included** robot tasks. Execute the following command to quickly train an ant to walk! -We recommend adding ``--headless`` for faster training. - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless - -... Or a robot dog! - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless +.. include:: include/src_python_virtual_env.rst -Isaac Lab provides the tools you'll need to create your own **Tasks** and **Workflows** for whatever your project needs may be. Take a look at our :ref:`how-to` guides like `Adding your own learning Library `_ or `Wrapping Environments `_ for details. +.. include:: include/src_build_isaaclab.rst -.. figure:: ../../_static/setup/isaac_ants_example.jpg - :align: center - :figwidth: 100% - :alt: Idle hands... +.. include:: include/src_verify_isaaclab.rst diff --git a/docs/source/setup/installation/cloud_installation.rst b/docs/source/setup/installation/cloud_installation.rst index 1f48a64b871..25572e74396 100644 --- a/docs/source/setup/installation/cloud_installation.rst +++ b/docs/source/setup/installation/cloud_installation.rst @@ -1,30 +1,54 @@ -Running Isaac Lab in the Cloud -============================== +Cloud Deployment +================ -Isaac Lab can be run in various cloud infrastructures with the use of `Isaac Automator `__. -Isaac Automator allows for quick deployment of Isaac Sim and Isaac Lab onto the public clouds (AWS, GCP, Azure, and Alibaba Cloud are currently supported). +Isaac Lab can be run in various cloud infrastructures with the use of +`Isaac Automator `__. -The result is a fully configured remote desktop cloud workstation, which can be used for development and testing of Isaac Lab within minutes and on a budget. Isaac Automator supports variety of GPU instances and stop-start functionality to save on cloud costs and a variety of tools to aid the workflow (like uploading and downloading data, autorun, deployment management, etc). +Isaac Automator allows for quick deployment of Isaac Sim and Isaac Lab onto +the public clouds (AWS, GCP, Azure, and Alibaba Cloud are currently supported). +The result is a fully configured remote desktop cloud workstation, which can +be used for development and testing of Isaac Lab within minutes and on a budget. +Isaac Automator supports variety of GPU instances and stop-start functionality +to save on cloud costs and a variety of tools to aid the workflow +(such as uploading and downloading data, autorun, deployment management, etc). + + +System Requirements +------------------- + +Isaac Automator requires having ``docker`` pre-installed on the system. + +* To install Docker, please follow the instructions for your operating system on the + `Docker website`_. A minimum version of 26.0.0 for Docker Engine and 2.25.0 for Docker + compose are required to work with Isaac Automator. +* Follow the post-installation steps for Docker on the `post-installation steps`_ page. + These steps allow you to run Docker without using ``sudo``. Installing Isaac Automator -------------------------- -For the most update-to-date and complete installation instructions, please refer to `Isaac Automator `__. +For the most update-to-date and complete installation instructions, please refer to +`Isaac Automator `__. To use Isaac Automator, first clone the repo: -.. code-block:: bash +.. tab-set:: - git clone https://github.com/isaac-sim/IsaacAutomator.git + .. tab-item:: HTTPS -Isaac Automator requires having ``docker`` pre-installed on the system. + .. code-block:: bash -* To install Docker, please follow the instructions for your operating system on the `Docker website`_. A minimum version of 26.0.0 for Docker Engine and 2.25.0 for Docker compose are required to work with Isaac Automator. -* Follow the post-installation steps for Docker on the `post-installation steps`_ page. These steps allow you to run - Docker without using ``sudo``. + git clone https://github.com/isaac-sim/IsaacAutomator.git -Isaac Automator also requires obtaining a NGC API key. + .. tab-item:: SSH + + .. code-block:: bash + + git clone git@github.com:isaac-sim/IsaacAutomator.git + + +Isaac Automator requires obtaining a NGC API key. * Get access to the `Isaac Sim container`_ by joining the NVIDIA Developer Program credentials. * Generate your `NGC API key`_ to access locked container images from NVIDIA GPU Cloud (NGC). @@ -46,8 +70,8 @@ Isaac Automator also requires obtaining a NGC API key. Password: -Running Isaac Automator ------------------------ +Building the container +---------------------- To run Isaac Automator, first build the Isaac Automator container: @@ -68,7 +92,14 @@ To run Isaac Automator, first build the Isaac Automator container: docker build --platform linux/x86_64 -t isa . -Next, enter the automator container: + +This will build the Isaac Automator container and tag it as ``isa``. + + +Running the Automator Commands +------------------------------ + +First, enter the Automator container: .. tab-set:: :sync-group: os @@ -87,22 +118,54 @@ Next, enter the automator container: docker run --platform linux/x86_64 -it --rm -v .:/app isa bash -Next, run the deployed script for your preferred cloud: +Next, run the deployment script for your preferred cloud: + +.. note:: + + The ``--isaaclab`` flag is used to specify the version of Isaac Lab to deploy. + The ``v2.3.0`` tag is the latest release of Isaac Lab. + +.. tab-set:: + :sync-group: cloud + + .. tab-item:: AWS + :sync: aws + + .. code-block:: bash + + ./deploy-aws --isaaclab v2.3.0 + + .. tab-item:: Azure + :sync: azure + + .. code-block:: bash -.. code-block:: bash + ./deploy-azure --isaaclab v2.3.0 - # AWS - ./deploy-aws - # Azure - ./deploy-azure - # GCP - ./deploy-gcp - # Alibaba Cloud - ./deploy-alicloud + .. tab-item:: GCP + :sync: gcp + + .. code-block:: bash + + ./deploy-gcp --isaaclab v2.3.0 + + .. tab-item:: Alibaba Cloud + :sync: alicloud + + .. code-block:: bash + + ./deploy-alicloud --isaaclab v2.3.0 Follow the prompts for entering information regarding the environment setup and credentials. -Once successful, instructions for connecting to the cloud instance will be available in the terminal. -Connections can be made using SSH, noVCN, or NoMachine. +Once successful, instructions for connecting to the cloud instance will be available +in the terminal. The deployed Isaac Sim instances can be accessed via: + +- SSH +- noVCN (browser-based VNC client) +- NoMachine (remote desktop client) + +Look for the connection instructions at the end of the deployment command output. +Additionally, this info is saved in ``state//info.txt`` file. For details on the credentials and setup required for each cloud, please visit the `Isaac Automator `__ @@ -133,16 +196,36 @@ For example: .. code-block:: batch - ./isaaclab.bat -p scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 + isaaclab.bat -p scripts/reinforcement_learning/rl_games/train.py --task=Isaac-Cartpole-v0 -Destroying a Development -------------------------- +Destroying a Deployment +----------------------- To save costs, deployments can be destroyed when not being used. -This can be done from within the Automator container, which can be entered with command ``./run``. +This can be done from within the Automator container. + +Enter the Automator container with the command described in the previous section: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + ./run + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + docker run --platform linux/x86_64 -it --rm -v .:/app isa bash + -To destroy a deployment, run: +To destroy a deployment, run the following command from within the container: .. code:: bash diff --git a/docs/source/setup/installation/include/bin_verify_isaacsim.rst b/docs/source/setup/installation/include/bin_verify_isaacsim.rst new file mode 100644 index 00000000000..19da95e1623 --- /dev/null +++ b/docs/source/setup/installation/include/bin_verify_isaacsim.rst @@ -0,0 +1,74 @@ +Check that the simulator runs as expected: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # note: you can pass the argument "--help" to see all arguments possible. + ${ISAACSIM_PATH}/isaac-sim.sh + + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: note: you can pass the argument "--help" to see all arguments possible. + %ISAACSIM_PATH%\isaac-sim.bat + + +Check that the simulator runs from a standalone python script: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # checks that python path is set correctly + ${ISAACSIM_PYTHON_EXE} -c "print('Isaac Sim configuration is now complete.')" + # checks that Isaac Sim can be launched from python + ${ISAACSIM_PYTHON_EXE} ${ISAACSIM_PATH}/standalone_examples/api/isaacsim.core.api/add_cubes.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: checks that python path is set correctly + %ISAACSIM_PYTHON_EXE% -c "print('Isaac Sim configuration is now complete.')" + :: checks that Isaac Sim can be launched from python + %ISAACSIM_PYTHON_EXE% %ISAACSIM_PATH%\standalone_examples\api\isaacsim.core.api\add_cubes.py + +.. caution:: + + If you have been using a previous version of Isaac Sim, you need to run the following command for the *first* + time after installation to remove all the old user data and cached variables: + + .. tab-set:: + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + + .. code:: bash + + ${ISAACSIM_PATH}/isaac-sim.sh --reset-user + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + + .. code:: batch + + %ISAACSIM_PATH%\isaac-sim.bat --reset-user + + +If the simulator does not run or crashes while following the above +instructions, it means that something is incorrectly configured. To +debug and troubleshoot, please check Isaac Sim +`documentation `__ +and the +`Isaac Sim Forums `_. diff --git a/docs/source/setup/installation/include/pip_python_virtual_env.rst b/docs/source/setup/installation/include/pip_python_virtual_env.rst new file mode 100644 index 00000000000..3586ef61cef --- /dev/null +++ b/docs/source/setup/installation/include/pip_python_virtual_env.rst @@ -0,0 +1,123 @@ +Preparing a Python Environment +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Creating a dedicated Python environment is **strongly recommended**. It helps: + +- **Avoid conflicts with system Python** or other projects installed on your machine. +- **Keep dependencies isolated**, so that package upgrades or experiments in other projects + do not break Isaac Sim. +- **Easily manage multiple environments** for setups with different versions of dependencies. +- **Simplify reproducibility** — the environment contains only the packages needed for the current project, + making it easier to share setups with colleagues or run on different machines. + +You can choose different package managers to create a virtual environment. + +- **UV**: A modern, fast, and secure package manager for Python. +- **Conda**: A cross-platform, language-agnostic package manager for Python. +- **venv**: The standard library for creating virtual environments in Python. + +.. caution:: + + The Python version of the virtual environment must match the Python version of Isaac Sim. + + - For Isaac Sim 5.X, the required Python version is 3.11. + - For Isaac Sim 4.X, the required Python version is 3.10. + + Using a different Python version will result in errors when running Isaac Lab. + +The following instructions are for Isaac Sim 5.X, which requires Python 3.11. +If you wish to install Isaac Sim 4.5, please use modify the instructions accordingly to use Python 3.10. + +- Create a virtual environment using one of the package managers: + + .. tab-set:: + + .. tab-item:: UV Environment + + To install ``uv``, please follow the instructions `here `__. + You can create the Isaac Lab environment using the following commands: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + # create a virtual environment named env_isaaclab with python3.11 + uv venv --python 3.11 env_isaaclab + # activate the virtual environment + source env_isaaclab/bin/activate + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + :: create a virtual environment named env_isaaclab with python3.11 + uv venv --python 3.11 env_isaaclab + :: activate the virtual environment + env_isaaclab\Scripts\activate + + .. tab-item:: Conda Environment + + To install conda, please follow the instructions `here `__. + You can create the Isaac Lab environment using the following commands. + + We recommend using `Miniconda `_, + since it is light-weight and resource-efficient environment management system. + + .. code-block:: bash + + conda create -n env_isaaclab python=3.11 + conda activate env_isaaclab + + .. tab-item:: venv Environment + + To create a virtual environment using the standard library, you can use the + following commands: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + # create a virtual environment named env_isaaclab with python3.11 + python3.11 -m venv env_isaaclab + # activate the virtual environment + source env_isaaclab/bin/activate + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + :: create a virtual environment named env_isaaclab with python3.11 + python3.11 -m venv env_isaaclab + :: activate the virtual environment + env_isaaclab\Scripts\activate + + +- Ensure the latest pip version is installed. To update pip, run the following command + from inside the virtual environment: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code-block:: bash + + pip install --upgrade pip + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code-block:: batch + + python -m pip install --upgrade pip diff --git a/docs/source/setup/installation/include/pip_verify_isaacsim.rst b/docs/source/setup/installation/include/pip_verify_isaacsim.rst new file mode 100644 index 00000000000..111b47d271b --- /dev/null +++ b/docs/source/setup/installation/include/pip_verify_isaacsim.rst @@ -0,0 +1,46 @@ + +Verifying the Isaac Sim installation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- Make sure that your virtual environment is activated (if applicable) + +- Check that the simulator runs as expected: + + .. code:: bash + + # note: you can pass the argument "--help" to see all arguments possible. + isaacsim + +- It's also possible to run with a specific experience file, run: + + .. code:: bash + + # experience files can be absolute path, or relative path searched in isaacsim/apps or omni/apps + isaacsim isaacsim.exp.full.kit + + +.. note:: + + When running Isaac Sim for the first time, all dependent extensions will be pulled from the registry. + This process can take upwards of 10 minutes and is required on the first run of each experience file. + Once the extensions are pulled, consecutive runs using the same experience file will use the cached extensions. + +.. attention:: + + The first run will prompt users to accept the Nvidia Omniverse License Agreement. + To accept the EULA, reply ``Yes`` when prompted with the below message: + + .. code:: bash + + By installing or using Isaac Sim, I agree to the terms of NVIDIA OMNIVERSE LICENSE AGREEMENT (EULA) + in https://docs.isaacsim.omniverse.nvidia.com/latest/common/NVIDIA_Omniverse_License_Agreement.html + + Do you accept the EULA? (Yes/No): Yes + + +If the simulator does not run or crashes while following the above +instructions, it means that something is incorrectly configured. To +debug and troubleshoot, please check Isaac Sim +`documentation `__ +and the +`Isaac Sim Forums `_. diff --git a/docs/source/setup/installation/include/src_build_isaaclab.rst b/docs/source/setup/installation/include/src_build_isaaclab.rst new file mode 100644 index 00000000000..ba822ae7b2c --- /dev/null +++ b/docs/source/setup/installation/include/src_build_isaaclab.rst @@ -0,0 +1,56 @@ +Installation +~~~~~~~~~~~~ + +- Install dependencies using ``apt`` (on Linux only): + + .. code:: bash + + # these dependency are needed by robomimic which is not available on Windows + sudo apt install cmake build-essential + +- Run the install command that iterates over all the extensions in ``source`` directory and installs them + using pip (with ``--editable`` flag): + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh --install # or "./isaaclab.sh -i" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat --install :: or "isaaclab.bat -i" + + + By default, the above will install **all** the learning frameworks. These include + ``rl_games``, ``rsl_rl``, ``sb3``, ``skrl``, ``robomimic``. + + If you want to install only a specific framework, you can pass the name of the framework + as an argument. For example, to install only the ``rl_games`` framework, you can run: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh --install rl_games # or "./isaaclab.sh -i rl_games" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat --install rl_games :: or "isaaclab.bat -i rl_games" + + The valid options are ``all``, ``rl_games``, ``rsl_rl``, ``sb3``, ``skrl``, ``robomimic``, + and ``none``. If ``none`` is passed, then no learning frameworks will be installed. diff --git a/docs/source/setup/installation/include/src_clone_isaaclab.rst b/docs/source/setup/installation/include/src_clone_isaaclab.rst new file mode 100644 index 00000000000..844cac2f3fd --- /dev/null +++ b/docs/source/setup/installation/include/src_clone_isaaclab.rst @@ -0,0 +1,78 @@ +Cloning Isaac Lab +~~~~~~~~~~~~~~~~~ + +.. note:: + + We recommend making a `fork `_ of the Isaac Lab repository to contribute + to the project but this is not mandatory to use the framework. If you + make a fork, please replace ``isaac-sim`` with your username + in the following instructions. + +Clone the Isaac Lab repository into your project's workspace: + +.. tab-set:: + + .. tab-item:: SSH + + .. code:: bash + + git clone git@github.com:isaac-sim/IsaacLab.git + + .. tab-item:: HTTPS + + .. code:: bash + + git clone https://github.com/isaac-sim/IsaacLab.git + + +We provide a helper executable `isaaclab.sh `_ +and `isaaclab.bat `_ for Linux and Windows +respectively that provides utilities to manage extensions. + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: text + + ./isaaclab.sh --help + + usage: isaaclab.sh [-h] [-i] [-f] [-p] [-s] [-t] [-o] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. + + optional arguments: + -h, --help Display the help content. + -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl_games, rsl_rl, sb3, skrl) as extra dependencies. Default is 'all'. + -f, --format Run pre-commit to format the code and check lints. + -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). + -s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim. + -t, --test Run all python pytest tests. + -o, --docker Run the docker container helper script (docker/container.sh). + -v, --vscode Generate the VSCode settings file from template. + -d, --docs Build the documentation from source using sphinx. + -n, --new Create a new external project or internal task from template. + -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. + -u, --uv [NAME] Create the uv environment for Isaac Lab. Default name is 'env_isaaclab'. + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: text + + isaaclab.bat --help + + usage: isaaclab.bat [-h] [-i] [-f] [-p] [-s] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. + + optional arguments: + -h, --help Display the help content. + -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl_games, rsl_rl, sb3, skrl) as extra dependencies. Default is 'all'. + -f, --format Run pre-commit to format the code and check lints. + -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). + -s, --sim Run the simulator executable (isaac-sim.bat) provided by Isaac Sim. + -t, --test Run all python pytest tests. + -v, --vscode Generate the VSCode settings file from template. + -d, --docs Build the documentation from source using sphinx. + -n, --new Create a new external project or internal task from template. + -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. + -u, --uv [NAME] Create the uv environment for Isaac Lab. Default name is 'env_isaaclab'. diff --git a/docs/source/setup/installation/include/src_python_virtual_env.rst b/docs/source/setup/installation/include/src_python_virtual_env.rst new file mode 100644 index 00000000000..d94d908d831 --- /dev/null +++ b/docs/source/setup/installation/include/src_python_virtual_env.rst @@ -0,0 +1,112 @@ +Setting up a Python Environment (optional) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. attention:: + This step is optional. If you are using the bundled Python with Isaac Sim, you can skip this step. + +Creating a dedicated Python environment for Isaac Lab is **strongly recommended**, even though +it is optional. Using a virtual environment helps: + +- **Avoid conflicts with system Python** or other projects installed on your machine. +- **Keep dependencies isolated**, so that package upgrades or experiments in other projects + do not break Isaac Sim. +- **Easily manage multiple environments** for setups with different versions of dependencies. +- **Simplify reproducibility** — the environment contains only the packages needed for the current project, + making it easier to share setups with colleagues or run on different machines. + + +You can choose different package managers to create a virtual environment. + +- **UV**: A modern, fast, and secure package manager for Python. +- **Conda**: A cross-platform, language-agnostic package manager for Python. + +Once created, you can use the default Python in the virtual environment (*python* or *python3*) +instead of *./isaaclab.sh -p* or *isaaclab.bat -p*. + +.. caution:: + + The Python version of the virtual environment must match the Python version of Isaac Sim. + + - For Isaac Sim 5.X, the required Python version is 3.11. + - For Isaac Sim 4.X, the required Python version is 3.10. + + Using a different Python version will result in errors when running Isaac Lab. + + +.. tab-set:: + + .. tab-item:: UV Environment + + To install ``uv``, please follow the instructions `here `__. + You can create the Isaac Lab environment using the following commands: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # Option 1: Default environment name 'env_isaaclab' + ./isaaclab.sh --uv # or "./isaaclab.sh -u" + # Option 2: Custom name + ./isaaclab.sh --uv my_env # or "./isaaclab.sh -u my_env" + + .. code:: bash + + # Activate environment + source ./env_isaaclab/bin/activate # or "source ./my_env/bin/activate" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. warning:: + Windows support for UV is currently unavailable. Please check + `issue #3483 `_ to track progress. + + .. tab-item:: Conda Environment + + To install conda, please follow the instructions `here `__. + You can create the Isaac Lab environment using the following commands. + + We recommend using `Miniconda `_, + since it is light-weight and resource-efficient environment management system. + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # Option 1: Default environment name 'env_isaaclab' + ./isaaclab.sh --conda # or "./isaaclab.sh -c" + # Option 2: Custom name + ./isaaclab.sh --conda my_env # or "./isaaclab.sh -c my_env" + + .. code:: bash + + # Activate environment + conda activate env_isaaclab # or "conda activate my_env" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: Option 1: Default environment name 'env_isaaclab' + isaaclab.bat --conda :: or "isaaclab.bat -c" + :: Option 2: Custom name + isaaclab.bat --conda my_env :: or "isaaclab.bat -c my_env" + + .. code:: batch + + :: Activate environment + conda activate env_isaaclab # or "conda activate my_env" + +Once you are in the virtual environment, you do not need to use ``./isaaclab.sh -p`` or +``isaaclab.bat -p`` to run python scripts. You can use the default python executable in your +environment by running ``python`` or ``python3``. However, for the rest of the documentation, +we will assume that you are using ``./isaaclab.sh -p`` or ``isaaclab.bat -p`` to run python scripts. diff --git a/docs/source/setup/installation/include/src_symlink_isaacsim.rst b/docs/source/setup/installation/include/src_symlink_isaacsim.rst new file mode 100644 index 00000000000..be8ae17cdbd --- /dev/null +++ b/docs/source/setup/installation/include/src_symlink_isaacsim.rst @@ -0,0 +1,43 @@ +Creating the Isaac Sim Symbolic Link +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Set up a symbolic link between the installed Isaac Sim root folder +and ``_isaac_sim`` in the Isaac Lab directory. This makes it convenient +to index the python modules and look for extensions shipped with Isaac Sim. + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # enter the cloned repository + cd IsaacLab + # create a symbolic link + ln -s ${ISAACSIM_PATH} _isaac_sim + + # For example: + # Option 1: If pre-built binaries were installed: + # ln -s ${HOME}/isaacsim _isaac_sim + # + # Option 2: If Isaac Sim was built from source: + # ln -s ${HOME}/IsaacSim/_build/linux-x86_64/release _isaac_sim + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: enter the cloned repository + cd IsaacLab + :: create a symbolic link - requires launching Command Prompt with Administrator access + mklink /D _isaac_sim %ISAACSIM_PATH% + + :: For example: + :: Option 1: If pre-built binaries were installed: + :: mklink /D _isaac_sim C:\isaacsim + :: + :: Option 2: If Isaac Sim was built from source: + :: mklink /D _isaac_sim C:\IsaacSim\_build\windows-x86_64\release diff --git a/docs/source/setup/installation/include/src_verify_isaaclab.rst b/docs/source/setup/installation/include/src_verify_isaaclab.rst new file mode 100644 index 00000000000..a747a1ccdc3 --- /dev/null +++ b/docs/source/setup/installation/include/src_verify_isaaclab.rst @@ -0,0 +1,102 @@ +Verifying the Isaac Lab installation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +To verify that the installation was successful, run the following command from the +top of the repository: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # Option 1: Using the isaaclab.sh executable + # note: this works for both the bundled python and the virtual environment + ./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py + + # Option 2: Using python in your virtual environment + python scripts/tutorials/00_sim/create_empty.py + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: Option 1: Using the isaaclab.bat executable + :: note: this works for both the bundled python and the virtual environment + isaaclab.bat -p scripts\tutorials\00_sim\create_empty.py + + :: Option 2: Using python in your virtual environment + python scripts\tutorials\00_sim\create_empty.py + + +The above command should launch the simulator and display a window with a black +viewport. You can exit the script by pressing ``Ctrl+C`` on your terminal. +On Windows machines, please terminate the process from Command Prompt using +``Ctrl+Break`` or ``Ctrl+fn+B``. + +.. figure:: /source/_static/setup/verify_install.jpg + :align: center + :figwidth: 100% + :alt: Simulator with a black window. + + +If you see this, then the installation was successful! |:tada:| + +.. note:: + + If you see an error ``ModuleNotFoundError: No module named 'isaacsim'``, please ensure that the virtual + environment is activated and ``source _isaac_sim/setup_conda_env.sh`` has been executed (for uv as well). + + +Train a robot! +~~~~~~~~~~~~~~ + +You can now use Isaac Lab to train a robot through Reinforcement Learning! The quickest way to use Isaac Lab is through the predefined workflows using one of our **Batteries-included** robot tasks. Execute the following command to quickly train an ant to walk! +We recommend adding ``--headless`` for faster training. + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless + +... Or a robot dog! + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless + +Isaac Lab provides the tools you'll need to create your own **Tasks** and **Workflows** for whatever your project needs may be. +Take a look at our :ref:`how-to` guides like :ref:`Adding your own learning Library ` or :ref:`Wrapping Environments ` for details. + +.. figure:: /source/_static/setup/isaac_ants_example.jpg + :align: center + :figwidth: 100% + :alt: Idle hands... diff --git a/docs/source/setup/installation/index.rst b/docs/source/setup/installation/index.rst index 432866bf5ce..f2aed3ef048 100644 --- a/docs/source/setup/installation/index.rst +++ b/docs/source/setup/installation/index.rst @@ -3,9 +3,9 @@ Local Installation ================== -.. image:: https://img.shields.io/badge/IsaacSim-5.0.0-silver.svg +.. image:: https://img.shields.io/badge/IsaacSim-5.1.0-silver.svg :target: https://developer.nvidia.com/isaac-sim - :alt: IsaacSim 5.0.0 + :alt: IsaacSim 5.1.0 .. image:: https://img.shields.io/badge/python-3.11-blue.svg :target: https://www.python.org/downloads/release/python-31013/ @@ -19,52 +19,178 @@ Local Installation :target: https://www.microsoft.com/en-ca/windows/windows-11 :alt: Windows 11 + +Isaac Lab installation is available for Windows and Linux. Since it is built on top of Isaac Sim, +it is required to install Isaac Sim before installing Isaac Lab. This guide explains the +recommended installation methods for both Isaac Sim and Isaac Lab. + .. caution:: We have dropped support for Isaac Sim versions 4.2.0 and below. We recommend using the latest - Isaac Sim 5.0.0 release to benefit from the latest features and improvements. + Isaac Sim 5.1.0 release to benefit from the latest features and improvements. For more information, please refer to the `Isaac Sim release notes `__. -.. note:: - We recommend system requirements with at least 32GB RAM and 16GB VRAM for Isaac Lab. - For workflows with rendering enabled, additional VRAM may be required. - For the full list of system requirements for Isaac Sim, please refer to the - `Isaac Sim system requirements `_. +System Requirements +------------------- - For details on driver requirements, please see the `Technical Requirements `_ guide +General Requirements +~~~~~~~~~~~~~~~~~~~~ - * See `Linux Troubleshooting `_ to resolve driver installation issues in linux - * If you are on a new GPU or are experiencing issues with the current drivers, we recommend installing the **latest production branch version** drivers from the `Unix Driver Archive `_ using the ``.run`` installer on Linux. - * NVIDIA driver version ``535.216.01`` or later is recommended when upgrading to **Ubuntu 22.04.5 kernel 6.8.0-48-generic** or later +For detailed requirements, please see the +`Isaac Sim system requirements `_. +The basic requirements are: +- **OS:** Ubuntu 22.04 (Linux x64) or Windows 11 (x64) +- **RAM:** 32 GB or more +- **GPU VRAM:** 16 GB or more (additional VRAM may be required for rendering workflows) -Isaac Lab is built on top of the Isaac Sim platform. Therefore, it is required to first install Isaac Sim -before using Isaac Lab. +**Isaac Sim is built against a specific Python version**, making +it essential to use the same Python version when installing Isaac Lab. +The required Python version is as follows: -Both Isaac Sim and Isaac Lab provide two ways of installation: -either through binary download/source file, or through Python's package installer ``pip``. +- For Isaac Sim 5.X, the required Python version is 3.11. +- For Isaac Sim 4.X, the required Python version is 3.10. -The method of installation may depend on the use case and the level of customization desired from users. -For example, installing Isaac Sim from pip will be a simpler process than installing it from binaries, -but the source code will then only be accessible through the installed source package and not through the direct binary download. -Similarly, installing Isaac Lab through pip is only recommended for workflows that use external launch scripts outside of Isaac Lab. -The Isaac Lab pip packages only provide the core framework extensions for Isaac Lab and does not include any of the -standalone training, inferencing, and example scripts. Therefore, this workflow is recommended for projects that are -built as external extensions outside of Isaac Lab, which utilizes user-defined runner scripts. +Driver Requirements +~~~~~~~~~~~~~~~~~~~ -We recommend using Isaac Sim pip installation for a simplified installation experience. +Drivers other than those recommended on `Omniverse Technical Requirements `_ +may work but have not been validated against all Omniverse tests. -For users getting started with Isaac Lab, we recommend installing Isaac Lab by cloning the repo. +- Use the **latest NVIDIA production branch driver**. +- On Linux, version ``580.65.06`` or later is recommended, especially when upgrading to + **Ubuntu 22.04.5 with kernel 6.8.0-48-generic** or newer. +- On Spark, version ``580.95.05`` is recommended. +- On Windows, version ``580.88`` is recommended. +- If you are using a new GPU or encounter driver issues, install the latest production branch + driver from the `Unix Driver Archive `_ + using the ``.run`` installer. +DGX Spark: details and limitations +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -.. toctree:: - :maxdepth: 2 +The DGX spark is a standalone machine learning device with aarch64 architecture. As a consequence, some +features of Isaac Lab are not currently supported on the DGX spark. The most noteworthy is that the architecture *requires* CUDA ≥ 13, and thus the cu13 build of PyTorch or newer. +Other notable limitations with respect to Isaac Lab include... + +#. `SkillGen `_ is not supported out of the box. This + is because cuRobo builds native CUDA/C++ extensions that requires specific tooling and library versions which are not validated for use with DGX spark. + +#. Extended reality teleoperation tools such as `OpenXR `_ is not supported. This is due + to encoding performance limitations that have not yet been fully investigated. + +#. SKRL training with JAX _ has not been explicitly validated or tested in Isaac Lab on the DGX Spark. + JAX provides pre-built CUDA wheels only for Linux on x86_64, so on aarch64 systems (e.g., DGX Spark) it runs on CPU only by default. + GPU support requires building JAX from source, which has not been validated in Isaac Lab. + +#. Livestream and Hub Workstation Cache are not supported on the DGX spark. + +#. Multi-node training may require direct connections between Spark machines or additional network configurations. + +#. :ref:`Isaac Lab Mimic ` data generation and policy inference for visuomotor environments are not supported on DGX Spark due to a lack of non-DLSS image denoiser on aarch64. + +#. :ref:`Running Cosmos Transfer1 ` is not currently supported on the DGX Spark. + +Troubleshooting +~~~~~~~~~~~~~~~ + +Please refer to the `Linux Troubleshooting `_ +to resolve installation issues in Linux. + +You can use `Isaac Sim Compatibility Checker `_ +to automatically check if the above requirements are met for running Isaac Sim on your system. + +Quick Start (Recommended) +------------------------- + +For most users, the simplest and fastest way to install Isaac Lab is by following the +:doc:`pip_installation` guide. + +This method will install Isaac Sim via pip and Isaac Lab through its source code. +If you are new to Isaac Lab, start here. + + +Choosing an Installation Method +------------------------------- + +Different workflows require different installation methods. +Use this table to decide: + ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| Method | Isaac Sim | Isaac Lab | Best For | Difficulty | ++===================+==============================+==============================+===========================+============+ +| **Recommended** | |:package:| pip install | |:floppy_disk:| source (git) | Beginners, standard use | Easy | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| Binary + Source | |:inbox_tray:| binary | |:floppy_disk:| source (git) | Users preferring binary | Easy | +| | download | | install of Isaac Sim | | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| Full Source Build | |:floppy_disk:| source (git) | |:floppy_disk:| source (git) | Developers modifying both | Advanced | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| Pip Only | |:package:| pip install | |:package:| pip install | External extensions only | Special | +| | | | (no training/examples) | case | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ +| Docker | |:whale:| Docker | |:floppy_disk:| source (git) | Docker users | Advanced | ++-------------------+------------------------------+------------------------------+---------------------------+------------+ - Pip installation (recommended) - Binary installation - Advanced installation (Isaac Lab pip) - Asset caching +Next Steps +---------- + +Once you've reviewed the installation methods, continue with the guide that matches your workflow: + +- |:smiley:| :doc:`pip_installation` + + - Install Isaac Sim via pip and Isaac Lab from source. + - Best for beginners and most users. + +- :doc:`binaries_installation` + + - Install Isaac Sim from its binary package (website download). + - Install Isaac Lab from its source code. + - Choose this if you prefer not to use pip for Isaac Sim (for instance, on Ubuntu 20.04). + +- :doc:`source_installation` + + - Build Isaac Sim from source. + - Install Isaac Lab from its source code. + - Recommended only if you plan to modify Isaac Sim itself. + +- :doc:`isaaclab_pip_installation` + + - Install Isaac Sim and Isaac Lab as pip packages. + - Best for advanced users building **external extensions** with custom runner scripts. + - Note: This does **not** include training or example scripts. + +- :ref:`container-deployment` + + - Install Isaac Sim and Isaac Lab in a Docker container. + - Best for users who want to use Isaac Lab in a containerized environment. + + +Asset Caching +------------- + +Isaac Lab assets are hosted on **AWS S3 cloud storage**. Loading times can vary +depending on your **network connection** and **geographical location**, and in some cases, +assets may take several minutes to load for each run. To improve performance or support +**offline workflows**, we recommend enabling **asset caching**. + +- Cached assets are stored locally, reducing repeated downloads. +- This is especially useful if you have a slow or intermittent internet connection, + or if your deployment environment is offline. + +Please follow the steps :doc:`asset_caching` to enable asset caching and speed up your workflow. + + +.. toctree:: + :maxdepth: 1 + :hidden: + + pip_installation + binaries_installation + source_installation + isaaclab_pip_installation + asset_caching diff --git a/docs/source/setup/installation/isaaclab_pip_installation.rst b/docs/source/setup/installation/isaaclab_pip_installation.rst index 2267e0cc5ec..b3fdd236653 100644 --- a/docs/source/setup/installation/isaaclab_pip_installation.rst +++ b/docs/source/setup/installation/isaaclab_pip_installation.rst @@ -1,5 +1,5 @@ -Installing Isaac Lab through Pip -================================ +Installation using Isaac Lab Pip Packages +========================================= From Isaac Lab 2.0, pip packages are provided to install both Isaac Sim and Isaac Lab extensions from pip. Note that this installation process is only recommended for advanced users working on additional extension projects @@ -7,148 +7,90 @@ that are built on top of Isaac Lab. Isaac Lab pip packages **does not** include training, inferencing, or running standalone workflows such as demos and examples. Therefore, users are required to define their own runner scripts when installing Isaac Lab from pip. -To learn about how to set up your own project on top of Isaac Lab, see :ref:`template-generator`. +To learn about how to set up your own project on top of Isaac Lab, please see :ref:`template-generator`. .. note:: - If you use Conda, we recommend using `Miniconda `_. - -- To use the pip installation approach for Isaac Lab, we recommend first creating a virtual environment. - Ensure that the python version of the virtual environment is **Python 3.11**. - - .. tab-set:: - - .. tab-item:: conda environment - - .. code-block:: bash - - conda create -n env_isaaclab python=3.11 - conda activate env_isaaclab - - .. tab-item:: venv environment - - .. tab-set:: - :sync-group: os + Currently, we only provide pip packages for every major release of Isaac Lab. + For example, we provide the pip package for release 2.1.0 and 2.2.0, but not 2.1.1. + In the future, we will provide pip packages for every minor release of Isaac Lab. - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux +.. include:: include/pip_python_virtual_env.rst - .. code-block:: bash +Installing dependencies +~~~~~~~~~~~~~~~~~~~~~~~ - # create a virtual environment named env_isaaclab with python3.11 - python3.11 -m venv env_isaaclab - # activate the virtual environment - source env_isaaclab/bin/activate +.. note:: - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows + In case you used UV to create your virtual environment, please replace ``pip`` with ``uv pip`` + in the following commands. - .. code-block:: batch +- Install the Isaac Lab packages along with Isaac Sim: - # create a virtual environment named env_isaaclab with python3.11 - python3.11 -m venv env_isaaclab - # activate the virtual environment - env_isaaclab\Scripts\activate + .. code-block:: none + pip install isaaclab[isaacsim,all]==2.3.0 --extra-index-url https://pypi.nvidia.com -- Before installing Isaac Lab, ensure the latest pip version is installed. To update pip, run +- Install a CUDA-enabled PyTorch build that matches your system architecture: .. tab-set:: - :sync-group: os + :sync-group: pip-platform - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux + .. tab-item:: :icon:`fa-brands fa-linux` Linux (x86_64) + :sync: linux-x86_64 .. code-block:: bash - pip install --upgrade pip - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code-block:: batch - - python -m pip install --upgrade pip - - -- Next, install a CUDA-enabled PyTorch 2.7.0 build for CUDA 12.8. - - .. code-block:: bash - - pip install torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 - + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 -- If using rl_games for training and inferencing, install the following python 3.11 enabled rl_games fork. - - .. code-block:: bash - - pip install git+https://github.com/isaac-sim/rl_games.git@python3.11 - -- Then, install the Isaac Lab packages, this will also install Isaac Sim. - - .. code-block:: none - - pip install isaaclab[isaacsim,all]==2.2.0 --extra-index-url https://pypi.nvidia.com - -.. note:: - - Currently, we only provide pip packages for every major release of Isaac Lab. - For example, we provide the pip package for release 2.1.0 and 2.2.0, but not 2.1.1. - In the future, we will provide pip packages for every minor release of Isaac Lab. - - -Verifying the Isaac Sim installation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -- Make sure that your virtual environment is activated (if applicable) + .. tab-item:: :icon:`fa-brands fa-windows` Windows (x86_64) + :sync: windows-x86_64 + .. code-block:: bash -- Check that the simulator runs as expected: + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 - .. code:: bash + .. tab-item:: :icon:`fa-brands fa-linux` Linux (aarch64) + :sync: linux-aarch64 - # note: you can pass the argument "--help" to see all arguments possible. - isaacsim + .. code-block:: bash -- It's also possible to run with a specific experience file, run: + pip install -U torch==2.9.0 torchvision==0.24.0 --index-url https://download.pytorch.org/whl/cu130 - .. code:: bash + .. note:: - # experience files can be absolute path, or relative path searched in isaacsim/apps or omni/apps - isaacsim isaacsim.exp.full.kit + After installing Isaac Lab on aarch64, you may encounter warnings such as: + .. code-block:: none -.. attention:: + ERROR: ld.so: object '...torch.libs/libgomp-XXXX.so.1.0.0' cannot be preloaded: ignored. - When running Isaac Sim for the first time, all dependent extensions will be pulled from the registry. - This process can take upwards of 10 minutes and is required on the first run of each experience file. - Once the extensions are pulled, consecutive runs using the same experience file will use the cached extensions. + This occurs when both the system and PyTorch ``libgomp`` (GNU OpenMP) libraries are preloaded. + Isaac Sim expects the **system** OpenMP runtime, while PyTorch sometimes bundles its own. -.. attention:: + To fix this, unset any existing ``LD_PRELOAD`` and set it to use the system library only: - The first run will prompt users to accept the Nvidia Omniverse License Agreement. - To accept the EULA, reply ``Yes`` when prompted with the below message: + .. code-block:: bash - .. code:: bash + unset LD_PRELOAD + export LD_PRELOAD="$LD_PRELOAD:/lib/aarch64-linux-gnu/libgomp.so.1" - By installing or using Isaac Sim, I agree to the terms of NVIDIA OMNIVERSE LICENSE AGREEMENT (EULA) - in https://docs.isaacsim.omniverse.nvidia.com/latest/common/NVIDIA_Omniverse_License_Agreement.html + This ensures the correct ``libgomp`` library is preloaded for both Isaac Sim and Isaac Lab, + removing the preload warnings during runtime. - Do you accept the EULA? (Yes/No): Yes +- If you want to use ``rl_games`` for training and inferencing, install + its Python 3.11 enabled fork: + .. code-block:: none -If the simulator does not run or crashes while following the above -instructions, it means that something is incorrectly configured. To -debug and troubleshoot, please check Isaac Sim -`documentation `__ -and the -`forums `__. + pip install git+https://github.com/isaac-sim/rl_games.git@python3.11 +.. include:: include/pip_verify_isaacsim.rst Running Isaac Lab Scripts ~~~~~~~~~~~~~~~~~~~~~~~~~ -By following the above scripts, your python environment should now have access to all of the Isaac Lab extensions. +By following the above scripts, your Python environment should now have access to all of the Isaac Lab extensions. To run a user-defined script for Isaac Lab, simply run .. code:: bash @@ -158,14 +100,17 @@ To run a user-defined script for Isaac Lab, simply run Generating VS Code Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Due to the structure resulting from the installation, VS Code IntelliSense (code completion, parameter info and member lists, etc.) will not work by default. -To set it up (define the search paths for import resolution, the path to the default Python interpreter, and other settings), for a given workspace folder, run the following command: +Due to the structure resulting from the installation, VS Code IntelliSense (code completion, parameter info +and member lists, etc.) will not work by default. To set it up (define the search paths for import resolution, +the path to the default Python interpreter, and other settings), for a given workspace folder, +run the following command: + +.. code-block:: bash - .. code-block:: bash + python -m isaaclab --generate-vscode-settings - python -m isaaclab --generate-vscode-settings - .. warning:: +.. warning:: - The command will generate a ``.vscode/settings.json`` file in the workspace folder. - If the file already exists, it will be overwritten (a confirmation prompt will be shown first). + The command will generate a ``.vscode/settings.json`` file in the workspace folder. + If the file already exists, it will be overwritten (a confirmation prompt will be shown first). diff --git a/docs/source/setup/installation/pip_installation.rst b/docs/source/setup/installation/pip_installation.rst index 48952959e59..5a6a5a7956d 100644 --- a/docs/source/setup/installation/pip_installation.rst +++ b/docs/source/setup/installation/pip_installation.rst @@ -1,390 +1,107 @@ .. _isaaclab-pip-installation: -Installation using Isaac Sim pip -================================ +Installation using Isaac Sim Pip Package +======================================== -Isaac Lab requires Isaac Sim. This tutorial first installs Isaac Sim from pip, then Isaac Lab from source code. - -Installing Isaac Sim --------------------- - -From Isaac Sim 4.0 release, it is possible to install Isaac Sim using pip. -This approach makes it easier to install Isaac Sim without requiring to download the Isaac Sim binaries. -If you encounter any issues, please report them to the -`Isaac Sim Forums `_. +The following steps first installs Isaac Sim from pip, then Isaac Lab from source code. .. attention:: Installing Isaac Sim with pip requires GLIBC 2.35+ version compatibility. To check the GLIBC version on your system, use command ``ldd --version``. - This may pose compatibility issues with some Linux distributions. For instance, Ubuntu 20.04 LTS has GLIBC 2.31 - by default. If you encounter compatibility issues, we recommend following the + This may pose compatibility issues with some Linux distributions. For instance, Ubuntu 20.04 LTS + has GLIBC 2.31 by default. If you encounter compatibility issues, we recommend following the :ref:`Isaac Sim Binaries Installation ` approach. -.. attention:: - - For details on driver requirements, please see the `Technical Requirements `_ guide! - - On Windows, it may be necessary to `enable long path support `_ to avoid installation errors due to OS limitations. - -.. attention:: +.. note:: If you plan to :ref:`Set up Visual Studio Code ` later, we recommend following the :ref:`Isaac Sim Binaries Installation ` approach. -.. note:: - - If you use Conda, we recommend using `Miniconda `_. - -- To use the pip installation approach for Isaac Sim, we recommend first creating a virtual environment. - Ensure that the python version of the virtual environment is **Python 3.11**. - - .. tab-set:: - - .. tab-item:: conda environment - - .. code-block:: bash +Installing Isaac Sim +-------------------- - conda create -n env_isaaclab python=3.11 - conda activate env_isaaclab +From Isaac Sim 4.0 onwards, it is possible to install Isaac Sim using pip. +This approach makes it easier to install Isaac Sim without requiring to download the Isaac Sim binaries. +If you encounter any issues, please report them to the +`Isaac Sim Forums `_. - .. tab-item:: venv environment +.. attention:: - .. tab-set:: - :sync-group: os + On Windows, it may be necessary to `enable long path support `_ + to avoid installation errors due to OS limitations. - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux +.. include:: include/pip_python_virtual_env.rst - .. code-block:: bash +Installing dependencies +~~~~~~~~~~~~~~~~~~~~~~~ - # create a virtual environment named env_isaaclab with python3.11 - python3.11 -m venv env_isaaclab - # activate the virtual environment - source env_isaaclab/bin/activate +.. note:: - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows + In case you used UV to create your virtual environment, please replace ``pip`` with ``uv pip`` + in the following commands. - .. code-block:: batch +- Install Isaac Sim pip packages: - # create a virtual environment named env_isaaclab with python3.11 - python3.11 -m venv env_isaaclab - # activate the virtual environment - env_isaaclab\Scripts\activate + .. code-block:: none + pip install "isaacsim[all,extscache]==5.1.0" --extra-index-url https://pypi.nvidia.com -- Before installing Isaac Sim, ensure the latest pip version is installed. To update pip, run +- Install a CUDA-enabled PyTorch build that matches your system architecture: .. tab-set:: - :sync-group: os + :sync-group: pip-platform - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux + .. tab-item:: :icon:`fa-brands fa-linux` Linux (x86_64) + :sync: linux-x86_64 .. code-block:: bash - pip install --upgrade pip - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code-block:: batch - - python -m pip install --upgrade pip - - -- Next, install a CUDA-enabled PyTorch 2.7.0 build. - - .. code-block:: bash - - pip install torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 + .. tab-item:: :icon:`fa-brands fa-windows` Windows (x86_64) + :sync: windows-x86_64 -- Then, install the Isaac Sim packages. - - .. code-block:: none - - pip install "isaacsim[all,extscache]==5.0.0" --extra-index-url https://pypi.nvidia.com - - -Verifying the Isaac Sim installation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -- Make sure that your virtual environment is activated (if applicable) - - -- Check that the simulator runs as expected: - - .. code:: bash - - # note: you can pass the argument "--help" to see all arguments possible. - isaacsim - -- It's also possible to run with a specific experience file, run: + .. code-block:: bash - .. code:: bash + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 - # experience files can be absolute path, or relative path searched in isaacsim/apps or omni/apps - isaacsim isaacsim.exp.full.kit + .. tab-item:: :icon:`fa-brands fa-linux` Linux (aarch64) + :sync: linux-aarch64 + .. code-block:: bash -.. attention:: + pip install -U torch==2.9.0 torchvision==0.24.0 --index-url https://download.pytorch.org/whl/cu130 - When running Isaac Sim for the first time, all dependent extensions will be pulled from the registry. - This process can take upwards of 10 minutes and is required on the first run of each experience file. - Once the extensions are pulled, consecutive runs using the same experience file will use the cached extensions. + .. note:: -.. attention:: + After installing Isaac Lab on aarch64, you may encounter warnings such as: - The first run will prompt users to accept the NVIDIA Software License Agreement. - To accept the EULA, reply ``Yes`` when prompted with the below message: + .. code-block:: none - .. code:: bash + ERROR: ld.so: object '...torch.libs/libgomp-XXXX.so.1.0.0' cannot be preloaded: ignored. - By installing or using Isaac Sim, I agree to the terms of NVIDIA SOFTWARE LICENSE AGREEMENT (EULA) - in https://www.nvidia.com/en-us/agreements/enterprise-software/nvidia-software-license-agreement + This occurs when both the system and PyTorch ``libgomp`` (GNU OpenMP) libraries are preloaded. + Isaac Sim expects the **system** OpenMP runtime, while PyTorch sometimes bundles its own. - Do you accept the EULA? (Yes/No): Yes + To fix this, unset any existing ``LD_PRELOAD`` and set it to use the system library only: + .. code-block:: bash -If the simulator does not run or crashes while following the above -instructions, it means that something is incorrectly configured. To -debug and troubleshoot, please check Isaac Sim -`documentation `__ -and the -`forums `__. + unset LD_PRELOAD + export LD_PRELOAD="$LD_PRELOAD:/lib/aarch64-linux-gnu/libgomp.so.1" + This ensures the correct ``libgomp`` library is preloaded for both Isaac Sim and Isaac Lab, + removing the preload warnings during runtime. +.. include:: include/pip_verify_isaacsim.rst Installing Isaac Lab -------------------- -Cloning Isaac Lab -~~~~~~~~~~~~~~~~~ - -.. note:: - - We recommend making a `fork `_ of the Isaac Lab repository to contribute - to the project but this is not mandatory to use the framework. If you - make a fork, please replace ``isaac-sim`` with your username - in the following instructions. - -Clone the Isaac Lab repository into your workspace: - -.. tab-set:: - - .. tab-item:: SSH - - .. code:: bash - - git clone git@github.com:isaac-sim/IsaacLab.git - - .. tab-item:: HTTPS - - .. code:: bash - - git clone https://github.com/isaac-sim/IsaacLab.git - - -.. note:: - We provide a helper executable `isaaclab.sh `_ that provides - utilities to manage extensions: - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: text - - ./isaaclab.sh --help - - usage: isaaclab.sh [-h] [-i] [-f] [-p] [-s] [-t] [-o] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. - - optional arguments: - -h, --help Display the help content. - -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl_games, rsl_rl, sb3, skrl) as extra dependencies. Default is 'all'. - -f, --format Run pre-commit to format the code and check lints. - -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). - -s, --sim Run the simulator executable (isaac-sim.sh) provided by Isaac Sim. - -t, --test Run all python pytest tests. - -o, --docker Run the docker container helper script (docker/container.sh). - -v, --vscode Generate the VSCode settings file from template. - -d, --docs Build the documentation from source using sphinx. - -n, --new Create a new external project or internal task from template. - -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: text - - isaaclab.bat --help - - usage: isaaclab.bat [-h] [-i] [-f] [-p] [-s] [-v] [-d] [-n] [-c] -- Utility to manage Isaac Lab. - - optional arguments: - -h, --help Display the help content. - -i, --install [LIB] Install the extensions inside Isaac Lab and learning frameworks (rl_games, rsl_rl, sb3, skrl) as extra dependencies. Default is 'all'. - -f, --format Run pre-commit to format the code and check lints. - -p, --python Run the python executable provided by Isaac Sim or virtual environment (if active). - -s, --sim Run the simulator executable (isaac-sim.bat) provided by Isaac Sim. - -t, --test Run all python pytest tests. - -v, --vscode Generate the VSCode settings file from template. - -d, --docs Build the documentation from source using sphinx. - -n, --new Create a new external project or internal task from template. - -c, --conda [NAME] Create the conda environment for Isaac Lab. Default name is 'env_isaaclab'. - -Installation -~~~~~~~~~~~~ - -- Install dependencies using ``apt`` (on Ubuntu): - - .. code:: bash - - sudo apt install cmake build-essential - -- Run the install command that iterates over all the extensions in ``source`` directory and installs them - using pip (with ``--editable`` flag): - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh --install # or "./isaaclab.sh -i" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: bash - - isaaclab.bat --install :: or "isaaclab.bat -i" - -.. note:: - - By default, this will install all the learning frameworks. If you want to install only a specific framework, you can - pass the name of the framework as an argument. For example, to install only the ``rl_games`` framework, you can run - - .. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh --install rl_games # or "./isaaclab.sh -i rl_games" - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: bash - - isaaclab.bat --install rl_games :: or "isaaclab.bat -i rl_games" - - The valid options are ``rl_games``, ``rsl_rl``, ``sb3``, ``skrl``, ``robomimic``, ``none``. - - -Verifying the Isaac Lab installation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -To verify that the installation was successful, run the following command from the -top of the repository: - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - # Option 1: Using the isaaclab.sh executable - # note: this works for both the bundled python and the virtual environment - ./isaaclab.sh -p scripts/tutorials/00_sim/create_empty.py - - # Option 2: Using python in your virtual environment - python scripts/tutorials/00_sim/create_empty.py - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - :: Option 1: Using the isaaclab.bat executable - :: note: this works for both the bundled python and the virtual environment - isaaclab.bat -p scripts\tutorials\00_sim\create_empty.py - - :: Option 2: Using python in your virtual environment - python scripts\tutorials\00_sim\create_empty.py - - -The above command should launch the simulator and display a window with a black -viewport as shown below. You can exit the script by pressing ``Ctrl+C`` on your terminal. -On Windows machines, please terminate the process from Command Prompt using -``Ctrl+Break`` or ``Ctrl+fn+B``. - - -.. figure:: ../../_static/setup/verify_install.jpg - :align: center - :figwidth: 100% - :alt: Simulator with a black window. - - -If you see this, then the installation was successful! |:tada:| - -Train a robot! -~~~~~~~~~~~~~~~ - -You can now use Isaac Lab to train a robot through Reinforcement Learning! The quickest way to use Isaac Lab is through the predefined workflows using one of our **Batteries-included** robot tasks. Execute the following command to quickly train an ant to walk! -We recommend adding ``--headless`` for faster training. - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Ant-v0 --headless - -... Or a robot dog! - -.. tab-set:: - :sync-group: os - - .. tab-item:: :icon:`fa-brands fa-linux` Linux - :sync: linux - - .. code:: bash - - ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless - - .. tab-item:: :icon:`fa-brands fa-windows` Windows - :sync: windows - - .. code:: batch - - isaaclab.bat -p scripts/reinforcement_learning/rsl_rl/train.py --task=Isaac-Velocity-Rough-Anymal-C-v0 --headless +.. include:: include/src_clone_isaaclab.rst -Isaac Lab provides the tools you'll need to create your own **Tasks** and **Workflows** for whatever your project needs may be. Take a look at our :ref:`how-to` guides like `Adding your own learning Library `_ or `Wrapping Environments `_ for details. +.. include:: include/src_build_isaaclab.rst -.. figure:: ../../_static/setup/isaac_ants_example.jpg - :align: center - :figwidth: 100% - :alt: Idle hands... +.. include:: include/src_verify_isaaclab.rst diff --git a/docs/source/setup/installation/source_installation.rst b/docs/source/setup/installation/source_installation.rst new file mode 100644 index 00000000000..c697c1dd205 --- /dev/null +++ b/docs/source/setup/installation/source_installation.rst @@ -0,0 +1,109 @@ +.. _isaaclab-source-installation: + +Installation using Isaac Sim Source Code +======================================== + +The following steps first installs Isaac Sim from source, then Isaac Lab from source code. + +.. note:: + + This is a more advanced installation method and is not recommended for most users. Only follow this method + if you wish to modify the source code of Isaac Sim as well. + +Installing Isaac Sim +-------------------- + +Building from source +~~~~~~~~~~~~~~~~~~~~ + +From Isaac Sim 5.0 release, it is possible to build Isaac Sim from its source code. +This approach is meant for users who wish to modify the source code of Isaac Sim as well, +or want to test Isaac Lab with the nightly version of Isaac Sim. + +The following instructions are adapted from the `Isaac Sim documentation `_ +for the convenience of users. + +.. attention:: + + Building Isaac Sim from source requires Ubuntu 22.04 LTS or higher. + +.. attention:: + + For details on driver requirements, please see the `Technical Requirements `_ guide! + + On Windows, it may be necessary to `enable long path support `_ to avoid installation errors due to OS limitations. + + +- Clone the Isaac Sim repository into your workspace: + + .. code:: bash + + git clone https://github.com/isaac-sim/IsaacSim.git + +- Build Isaac Sim from source: + + .. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + cd IsaacSim + ./build.sh + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: bash + + cd IsaacSim + build.bat + + +Verifying the Isaac Sim installation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +To avoid the overhead of finding and locating the Isaac Sim installation +directory every time, we recommend exporting the following environment +variables to your terminal for the remaining of the installation instructions: + +.. tab-set:: + :sync-group: os + + .. tab-item:: :icon:`fa-brands fa-linux` Linux + :sync: linux + + .. code:: bash + + # Isaac Sim root directory + export ISAACSIM_PATH="${pwd}/_build/linux-x86_64/release" + # Isaac Sim python executable + export ISAACSIM_PYTHON_EXE="${ISAACSIM_PATH}/python.sh" + + .. tab-item:: :icon:`fa-brands fa-windows` Windows + :sync: windows + + .. code:: batch + + :: Isaac Sim root directory + set ISAACSIM_PATH="%cd%\_build\windows-x86_64\release" + :: Isaac Sim python executable + set ISAACSIM_PYTHON_EXE="%ISAACSIM_PATH:"=%\python.bat" + +.. include:: include/bin_verify_isaacsim.rst + + +Installing Isaac Lab +-------------------- + +.. include:: include/src_clone_isaaclab.rst + +.. include:: include/src_symlink_isaacsim.rst + +.. include:: include/src_python_virtual_env.rst + +.. include:: include/src_build_isaaclab.rst + +.. include:: include/src_verify_isaaclab.rst diff --git a/docs/source/setup/quickstart.rst b/docs/source/setup/quickstart.rst index a42bc665570..5c291de29af 100644 --- a/docs/source/setup/quickstart.rst +++ b/docs/source/setup/quickstart.rst @@ -42,7 +42,7 @@ Next, install a CUDA-enabled PyTorch 2.7.0 build. .. code-block:: bash - pip install torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 + pip install -U torch==2.7.0 torchvision==0.22.0 --index-url https://download.pytorch.org/whl/cu128 Before we can install Isaac Sim, we need to make sure pip is updated. To update pip, run @@ -68,7 +68,7 @@ and now we can install the Isaac Sim packages. .. code-block:: none - pip install "isaacsim[all,extscache]==5.0.0" --extra-index-url https://pypi.nvidia.com + pip install "isaacsim[all,extscache]==5.1.0" --extra-index-url https://pypi.nvidia.com Finally, we can install Isaac Lab. To start, clone the repository using the following diff --git a/docs/source/setup/walkthrough/technical_env_design.rst b/docs/source/setup/walkthrough/technical_env_design.rst index f1774a2804a..982a579f683 100644 --- a/docs/source/setup/walkthrough/technical_env_design.rst +++ b/docs/source/setup/walkthrough/technical_env_design.rst @@ -35,7 +35,7 @@ The contents of ``jetbot.py`` is fairly minimal from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR JETBOT_CONFIG = ArticulationCfg( - spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Jetbot/jetbot.usd"), + spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/NVIDIA/Jetbot/jetbot.usd"), actuators={"wheel_acts": ImplicitActuatorCfg(joint_names_expr=[".*"], damping=None, stiffness=None)}, ) diff --git a/docs/source/tutorials/00_sim/launch_app.rst b/docs/source/tutorials/00_sim/launch_app.rst index 8013e9975c3..05fa32c4648 100644 --- a/docs/source/tutorials/00_sim/launch_app.rst +++ b/docs/source/tutorials/00_sim/launch_app.rst @@ -172,5 +172,5 @@ want our simulation to be more performant. The process can be killed by pressing terminal. -.. _specification: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.simulation_app/docs/index.html#isaacsim.simulation_app.SimulationApp.DEFAULT_LAUNCHER_CONFIG +.. _specification: https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.simulation_app/docs/index.html#isaacsim.simulation_app.SimulationApp.DEFAULT_LAUNCHER_CONFIG .. _WebRTC Livestreaming: https://docs.isaacsim.omniverse.nvidia.com/latest/installation/manual_livestream_clients.html#isaac-sim-short-webrtc-streaming-client diff --git a/docs/source/tutorials/01_assets/add_new_robot.rst b/docs/source/tutorials/01_assets/add_new_robot.rst index 61664cef518..a4d258f82c1 100644 --- a/docs/source/tutorials/01_assets/add_new_robot.rst +++ b/docs/source/tutorials/01_assets/add_new_robot.rst @@ -6,7 +6,7 @@ Adding a New Robot to Isaac Lab .. currentmodule:: isaaclab Simulating and training a new robot is a multi-step process that starts with importing the robot into Isaac Sim. -This is covered in depth in the Isaac Sim documentation `here `_. +This is covered in depth in the Isaac Sim documentation `here `_. Once the robot is imported and tuned for simulation, we must define those interfaces necessary to clone the robot across multiple environments, drive its joints, and properly reset it, regardless of the chosen workflow or training framework. diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout new file mode 100644 index 00000000000..3aad5336af1 --- /dev/null +++ b/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE new file mode 100644 index 00000000000..e69de29bb2d diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py new file mode 100644 index 00000000000..3c6d9e87790 --- /dev/null +++ b/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py new file mode 100644 index 00000000000..f67eaa9891e --- /dev/null +++ b/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/__init__.py new file mode 100644 index 00000000000..8fc6d9b020a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/__init__.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package containing the core framework.""" + +import os +import toml + +# Conveniences to other module directories via relative paths +ISAACLAB_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +"""Path to the extension source directory.""" + +ISAACLAB_METADATA = toml.load(os.path.join(ISAACLAB_EXT_DIR, "config", "extension.toml")) +"""Extension metadata dictionary parsed from the extension.toml file.""" + +# Configure the module-level variables +__version__ = ISAACLAB_METADATA["package"]["version"] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/__init__.py new file mode 100644 index 00000000000..bc577bf51ec --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/__init__.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for different actuator models. + +Actuator models are used to model the behavior of the actuators in an articulation. These +are usually meant to be used in simulation to model different actuator dynamics and delays. + +There are two main categories of actuator models that are supported: + +- **Implicit**: Motor model with ideal PD from the physics engine. This is similar to having a continuous time + PD controller. The motor model is implicit in the sense that the motor model is not explicitly defined by the user. +- **Explicit**: Motor models based on physical drive models. + + - **Physics-based**: Derives the motor models based on first-principles. + - **Neural Network-based**: Learned motor models from actuator data. + +Every actuator model inherits from the :class:`isaaclab.actuators.ActuatorBase` class, +which defines the common interface for all actuator models. The actuator models are handled +and called by the :class:`isaaclab.assets.Articulation` class. +""" + +from .actuator_base import ActuatorBase +from .actuator_cfg import ( + ActuatorBaseCfg, + ActuatorNetLSTMCfg, + ActuatorNetMLPCfg, + DCMotorCfg, + DelayedPDActuatorCfg, + IdealPDActuatorCfg, + ImplicitActuatorCfg, + RemotizedPDActuatorCfg, +) +from .actuator_net import ActuatorNetLSTM, ActuatorNetMLP +from .actuator_pd import DCMotor, DelayedPDActuator, IdealPDActuator, ImplicitActuator, RemotizedPDActuator diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_base.py new file mode 100644 index 00000000000..e6fd231872d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_base.py @@ -0,0 +1,323 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from abc import ABC, abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING, ClassVar + +import isaaclab.utils.string as string_utils +from isaaclab.utils.types import ArticulationActions + +if TYPE_CHECKING: + from .actuator_cfg import ActuatorBaseCfg + + +class ActuatorBase(ABC): + """Base class for actuator models over a collection of actuated joints in an articulation. + + Actuator models augment the simulated articulation joints with an external drive dynamics model. + The model is used to convert the user-provided joint commands (positions, velocities and efforts) + into the desired joint positions, velocities and efforts that are applied to the simulated articulation. + + The base class provides the interface for the actuator models. It is responsible for parsing the + actuator parameters from the configuration and storing them as buffers. It also provides the + interface for resetting the actuator state and computing the desired joint commands for the simulation. + + For each actuator model, a corresponding configuration class is provided. The configuration class + is used to parse the actuator parameters from the configuration. It also specifies the joint names + for which the actuator model is applied. These names can be specified as regular expressions, which + are matched against the joint names in the articulation. + + To see how the class is used, check the :class:`isaaclab.assets.Articulation` class. + """ + + is_implicit_model: ClassVar[bool] = False + """Flag indicating if the actuator is an implicit or explicit actuator model. + + If a class inherits from :class:`ImplicitActuator`, then this flag should be set to :obj:`True`. + """ + + computed_effort: torch.Tensor + """The computed effort for the actuator group. Shape is (num_envs, num_joints).""" + + applied_effort: torch.Tensor + """The applied effort for the actuator group. Shape is (num_envs, num_joints). + + This is the effort obtained after clipping the :attr:`computed_effort` based on the + actuator characteristics. + """ + + effort_limit: torch.Tensor + """The effort limit for the actuator group. Shape is (num_envs, num_joints). + + For implicit actuators, the :attr:`effort_limit` and :attr:`effort_limit_sim` are the same. + """ + + effort_limit_sim: torch.Tensor + """The effort limit for the actuator group in the simulation. Shape is (num_envs, num_joints). + + For implicit actuators, the :attr:`effort_limit` and :attr:`effort_limit_sim` are the same. + """ + + velocity_limit: torch.Tensor + """The velocity limit for the actuator group. Shape is (num_envs, num_joints). + + For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same. + """ + + velocity_limit_sim: torch.Tensor + """The velocity limit for the actuator group in the simulation. Shape is (num_envs, num_joints). + + For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same. + """ + + stiffness: torch.Tensor + """The stiffness (P gain) of the PD controller. Shape is (num_envs, num_joints).""" + + damping: torch.Tensor + """The damping (D gain) of the PD controller. Shape is (num_envs, num_joints).""" + + armature: torch.Tensor + """The armature of the actuator joints. Shape is (num_envs, num_joints).""" + + friction: torch.Tensor + """The joint friction of the actuator joints. Shape is (num_envs, num_joints).""" + + _DEFAULT_MAX_EFFORT_SIM: ClassVar[float] = 1.0e9 + """The default maximum effort for the actuator joints in the simulation. Defaults to 1.0e9. + + If the :attr:`ActuatorBaseCfg.effort_limit_sim` is not specified and the actuator is an explicit + actuator, then this value is used. + """ + + def __init__( + self, + cfg: ActuatorBaseCfg, + joint_names: list[str], + joint_ids: slice | torch.Tensor, + num_envs: int, + device: str, + stiffness: torch.Tensor | float = 0.0, + damping: torch.Tensor | float = 0.0, + armature: torch.Tensor | float = 0.0, + friction: torch.Tensor | float = 0.0, + effort_limit: torch.Tensor | float = torch.inf, + velocity_limit: torch.Tensor | float = torch.inf, + ): + """Initialize the actuator. + + The actuator parameters are parsed from the configuration and stored as buffers. If the parameters + are not specified in the configuration, then their values provided in the constructor are used. + + .. note:: + The values in the constructor are typically obtained through the USD schemas corresponding + to the joints in the actuator model. + + Args: + cfg: The configuration of the actuator model. + joint_names: The joint names in the articulation. + joint_ids: The joint indices in the articulation. If :obj:`slice(None)`, then all + the joints in the articulation are part of the group. + num_envs: Number of articulations in the view. + device: Device used for processing. + stiffness: The default joint stiffness (P gain). Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + damping: The default joint damping (D gain). Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + armature: The default joint armature. Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + friction: The default joint friction. Defaults to 0.0. + If a tensor, then the shape is (num_envs, num_joints). + effort_limit: The default effort limit. Defaults to infinity. + If a tensor, then the shape is (num_envs, num_joints). + velocity_limit: The default velocity limit. Defaults to infinity. + If a tensor, then the shape is (num_envs, num_joints). + """ + # save parameters + self.cfg = cfg + self._num_envs = num_envs + self._device = device + self._joint_names = joint_names + self._joint_indices = joint_ids + + # For explicit models, we do not want to enforce the effort limit through the solver + # (unless it is explicitly set) + if not self.is_implicit_model and self.cfg.effort_limit_sim is None: + self.cfg.effort_limit_sim = self._DEFAULT_MAX_EFFORT_SIM + + # parse joint stiffness and damping + self.stiffness = self._parse_joint_parameter(self.cfg.stiffness, stiffness) + self.damping = self._parse_joint_parameter(self.cfg.damping, damping) + # parse joint armature and friction + self.armature = self._parse_joint_parameter(self.cfg.armature, armature) + self.friction = self._parse_joint_parameter(self.cfg.friction, friction) + # parse joint limits + # -- velocity + self.velocity_limit_sim = self._parse_joint_parameter(self.cfg.velocity_limit_sim, velocity_limit) + self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, self.velocity_limit_sim) + # -- effort + self.effort_limit_sim = self._parse_joint_parameter(self.cfg.effort_limit_sim, effort_limit) + self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, self.effort_limit_sim) + + # create commands buffers for allocation + self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device) + self.applied_effort = torch.zeros_like(self.computed_effort) + + def __str__(self) -> str: + """Returns: A string representation of the actuator group.""" + # resolve joint indices for printing + joint_indices = self.joint_indices + if joint_indices == slice(None): + joint_indices = list(range(self.num_joints)) + # resolve model type (implicit or explicit) + model_type = "implicit" if self.is_implicit_model else "explicit" + + return ( + f" object:\n" + f"\tModel type : {model_type}\n" + f"\tNumber of joints : {self.num_joints}\n" + f"\tJoint names expression: {self.cfg.joint_names_expr}\n" + f"\tJoint names : {self.joint_names}\n" + f"\tJoint indices : {joint_indices}\n" + ) + + """ + Properties. + """ + + @property + def num_joints(self) -> int: + """Number of actuators in the group.""" + return len(self._joint_names) + + @property + def joint_names(self) -> list[str]: + """Articulation's joint names that are part of the group.""" + return self._joint_names + + @property + def joint_indices(self) -> slice | torch.Tensor: + """Articulation's joint indices that are part of the group. + + Note: + If :obj:`slice(None)` is returned, then the group contains all the joints in the articulation. + We do this to avoid unnecessary indexing of the joints for performance reasons. + """ + return self._joint_indices + + """ + Operations. + """ + + @abstractmethod + def reset(self, env_ids: Sequence[int]): + """Reset the internals within the group. + + Args: + env_ids: List of environment IDs to reset. + """ + raise NotImplementedError + + @abstractmethod + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + """Process the actuator group actions and compute the articulation actions. + + It computes the articulation actions based on the actuator model type + + Args: + control_action: The joint action instance comprising of the desired joint positions, joint velocities + and (feed-forward) joint efforts. + joint_pos: The current joint positions of the joints in the group. Shape is (num_envs, num_joints). + joint_vel: The current joint velocities of the joints in the group. Shape is (num_envs, num_joints). + + Returns: + The computed desired joint positions, joint velocities and joint efforts. + """ + raise NotImplementedError + + """ + Helper functions. + """ + + def _parse_joint_parameter( + self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None + ) -> torch.Tensor: + """Parse the joint parameter from the configuration. + + Args: + cfg_value: The parameter value from the configuration. If None, then use the default value. + default_value: The default value to use if the parameter is None. If it is also None, + then an error is raised. + + Returns: + The parsed parameter value. + + Raises: + TypeError: If the parameter value is not of the expected type. + TypeError: If the default value is not of the expected type. + ValueError: If the parameter value is None and no default value is provided. + ValueError: If the default value tensor is the wrong shape. + """ + # create parameter buffer + param = torch.zeros(self._num_envs, self.num_joints, device=self._device) + # parse the parameter + if cfg_value is not None: + if isinstance(cfg_value, (float, int)): + # if float, then use the same value for all joints + param[:] = float(cfg_value) + elif isinstance(cfg_value, dict): + # if dict, then parse the regular expression + indices, _, values = string_utils.resolve_matching_names_values(cfg_value, self.joint_names) + # note: need to specify type to be safe (e.g. values are ints, but we want floats) + param[:, indices] = torch.tensor(values, dtype=torch.float, device=self._device) + else: + raise TypeError( + f"Invalid type for parameter value: {type(cfg_value)} for " + + f"actuator on joints {self.joint_names}. Expected float or dict." + ) + elif default_value is not None: + if isinstance(default_value, (float, int)): + # if float, then use the same value for all joints + param[:] = float(default_value) + elif isinstance(default_value, torch.Tensor): + # if tensor, then use the same tensor for all joints + if default_value.shape == (self._num_envs, self.num_joints): + param = default_value.float() + else: + raise ValueError( + "Invalid default value tensor shape.\n" + f"Got: {default_value.shape}\n" + f"Expected: {(self._num_envs, self.num_joints)}" + ) + else: + raise TypeError( + f"Invalid type for default value: {type(default_value)} for " + + f"actuator on joints {self.joint_names}. Expected float or Tensor." + ) + else: + raise ValueError("The parameter value is None and no default value is provided.") + + return param + + def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: + """Clip the desired torques based on the motor limits. + + Args: + desired_torques: The desired torques to clip. + + Returns: + The clipped torques. + """ + return torch.clip(effort, min=-self.effort_limit, max=self.effort_limit) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_cfg.py new file mode 100644 index 00000000000..d61bc6eaec5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_cfg.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Iterable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import actuator_net, actuator_pd +from .actuator_base import ActuatorBase + + +@configclass +class ActuatorBaseCfg: + """Configuration for default actuators in an articulation.""" + + class_type: type[ActuatorBase] = MISSING + """The associated actuator class. + + The class should inherit from :class:`isaaclab.actuators.ActuatorBase`. + """ + + joint_names_expr: list[str] = MISSING + """Articulation's joint names that are part of the group. + + Note: + This can be a list of joint names or a list of regex expressions (e.g. ".*"). + """ + + effort_limit: dict[str, float] | float | None = None + """Force/Torque limit of the joints in the group. Defaults to None. + + This limit is used to clip the computed torque sent to the simulation. If None, the + limit is set to the value specified in the USD joint prim. + + .. attention:: + + The :attr:`effort_limit_sim` attribute should be used to set the effort limit for + the simulation physics solver. + + The :attr:`effort_limit` attribute is used for clipping the effort output of the + actuator model **only** in the case of explicit actuators, such as the + :class:`~isaaclab.actuators.IdealPDActuator`. + + .. note:: + + For implicit actuators, the attributes :attr:`effort_limit` and :attr:`effort_limit_sim` + are equivalent. However, we suggest using the :attr:`effort_limit_sim` attribute because + it is more intuitive. + + """ + + velocity_limit: dict[str, float] | float | None = None + """Velocity limit of the joints in the group. Defaults to None. + + This limit is used by the actuator model. If None, the limit is set to the value specified + in the USD joint prim. + + .. attention:: + + The :attr:`velocity_limit_sim` attribute should be used to set the velocity limit for + the simulation physics solver. + + The :attr:`velocity_limit` attribute is used for clipping the effort output of the + actuator model **only** in the case of explicit actuators, such as the + :class:`~isaaclab.actuators.IdealPDActuator`. + + .. note:: + + For implicit actuators, the attribute :attr:`velocity_limit` is not used. This is to stay + backwards compatible with previous versions of the Isaac Lab, where this parameter was + unused since PhysX did not support setting the velocity limit for the joints using the + PhysX Tensor API. + """ + + effort_limit_sim: dict[str, float] | float | None = None + """Effort limit of the joints in the group applied to the simulation physics solver. Defaults to None. + + The effort limit is used to constrain the computed joint efforts in the physics engine. If the + computed effort exceeds this limit, the physics engine will clip the effort to this value. + + Since explicit actuators (e.g. DC motor), compute and clip the effort in the actuator model, this + limit is by default set to a large value to prevent the physics engine from any additional clipping. + However, at times, it may be necessary to set this limit to a smaller value as a safety measure. + + If None, the limit is resolved based on the type of actuator model: + + * For implicit actuators, the limit is set to the value specified in the USD joint prim. + * For explicit actuators, the limit is set to 1.0e9. + + """ + + velocity_limit_sim: dict[str, float] | float | None = None + """Velocity limit of the joints in the group applied to the simulation physics solver. Defaults to None. + + The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only + be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving + faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity. + + If None, the limit is set to the value specified in the USD joint prim for both implicit and explicit actuators. + + .. tip:: + If the velocity limit is too tight, the physics engine may have trouble converging to a solution. + In such cases, we recommend either keeping this value sufficiently large or tuning the stiffness and + damping parameters of the joint to ensure the limits are not violated. + + """ + + stiffness: dict[str, float] | float | None = MISSING + """Stiffness gains (also known as p-gain) of the joints in the group. + + The behavior of the stiffness is different for implicit and explicit actuators. For implicit actuators, + the stiffness gets set into the physics engine directly. For explicit actuators, the stiffness is used + by the actuator model to compute the joint efforts. + + If None, the stiffness is set to the value from the USD joint prim. + """ + + damping: dict[str, float] | float | None = MISSING + """Damping gains (also known as d-gain) of the joints in the group. + + The behavior of the damping is different for implicit and explicit actuators. For implicit actuators, + the damping gets set into the physics engine directly. For explicit actuators, the damping gain is used + by the actuator model to compute the joint efforts. + + If None, the damping is set to the value from the USD joint prim. + """ + + armature: dict[str, float] | float | None = None + """Armature of the joints in the group. Defaults to None. + + The armature is directly added to the corresponding joint-space inertia. It helps improve the + simulation stability by reducing the joint velocities. + + It is a physics engine solver parameter that gets set into the simulation. + + If None, the armature is set to the value from the USD joint prim. + """ + + friction: dict[str, float] | float | None = None + r"""The friction coefficient of the joints in the group. Defaults to None. + + The joint friction is a unitless quantity. It relates the magnitude of the spatial force transmitted + from the parent body to the child body to the maximal friction force that may be applied by the solver + to resist the joint motion. + + Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` + is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force + transmitted from the parent body to the child body. The simulated friction effect is therefore + similar to static and Coulomb friction. + + If None, the joint friction is set to the value from the USD joint prim. + """ + + +""" +Implicit Actuator Models. +""" + + +@configclass +class ImplicitActuatorCfg(ActuatorBaseCfg): + """Configuration for an implicit actuator. + + Note: + The PD control is handled implicitly by the simulation. + """ + + class_type: type = actuator_pd.ImplicitActuator + + +""" +Explicit Actuator Models. +""" + + +@configclass +class IdealPDActuatorCfg(ActuatorBaseCfg): + """Configuration for an ideal PD actuator.""" + + class_type: type = actuator_pd.IdealPDActuator + + +@configclass +class DCMotorCfg(IdealPDActuatorCfg): + """Configuration for direct control (DC) motor actuator model.""" + + class_type: type = actuator_pd.DCMotor + + saturation_effort: float = MISSING + """Peak motor force/torque of the electric DC motor (in N-m).""" + + +@configclass +class ActuatorNetLSTMCfg(DCMotorCfg): + """Configuration for LSTM-based actuator model.""" + + class_type: type = actuator_net.ActuatorNetLSTM + # we don't use stiffness and damping for actuator net + stiffness = None + damping = None + + network_file: str = MISSING + """Path to the file containing network weights.""" + + +@configclass +class ActuatorNetMLPCfg(DCMotorCfg): + """Configuration for MLP-based actuator model.""" + + class_type: type = actuator_net.ActuatorNetMLP + # we don't use stiffness and damping for actuator net + stiffness = None + damping = None + + network_file: str = MISSING + """Path to the file containing network weights.""" + + pos_scale: float = MISSING + """Scaling of the joint position errors input to the network.""" + vel_scale: float = MISSING + """Scaling of the joint velocities input to the network.""" + torque_scale: float = MISSING + """Scaling of the joint efforts output from the network.""" + + input_order: Literal["pos_vel", "vel_pos"] = MISSING + """Order of the inputs to the network. + + The order can be one of the following: + + * ``"pos_vel"``: joint position errors followed by joint velocities + * ``"vel_pos"``: joint velocities followed by joint position errors + """ + + input_idx: Iterable[int] = MISSING + """ + Indices of the actuator history buffer passed as inputs to the network. + + The index *0* corresponds to current time-step, while *n* corresponds to n-th + time-step in the past. The allocated history length is `max(input_idx) + 1`. + """ + + +@configclass +class DelayedPDActuatorCfg(IdealPDActuatorCfg): + """Configuration for a delayed PD actuator.""" + + class_type: type = actuator_pd.DelayedPDActuator + + min_delay: int = 0 + """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + + max_delay: int = 0 + """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" + + +@configclass +class RemotizedPDActuatorCfg(DelayedPDActuatorCfg): + """Configuration for a remotized PD actuator. + + Note: + The torque output limits for this actuator is derived from a linear interpolation of a lookup table + in :attr:`joint_parameter_lookup`. This table describes the relationship between joint angles and + the output torques. + """ + + class_type: type = actuator_pd.RemotizedPDActuator + + joint_parameter_lookup: list[list[float]] = MISSING + """Joint parameter lookup table. Shape is (num_lookup_points, 3). + + This tensor describes the relationship between the joint angle (rad), the transmission ratio (in/out), + and the output torque (N*m). The table is used to interpolate the output torque based on the joint angle. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_net.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_net.py new file mode 100644 index 00000000000..91342df1e31 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_net.py @@ -0,0 +1,194 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Neural network models for actuators. + +Currently, the following models are supported: + +* Multi-Layer Perceptron (MLP) +* Long Short-Term Memory (LSTM) + +""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.utils.assets import read_file +from isaaclab.utils.types import ArticulationActions + +from .actuator_pd import DCMotor + +if TYPE_CHECKING: + from .actuator_cfg import ActuatorNetLSTMCfg, ActuatorNetMLPCfg + + +class ActuatorNetLSTM(DCMotor): + """Actuator model based on recurrent neural network (LSTM). + + Unlike the MLP implementation :cite:t:`hwangbo2019learning`, this class implements + the learned model as a temporal neural network (LSTM) based on the work from + :cite:t:`rudin2022learning`. This removes the need of storing a history as the + hidden states of the recurrent network captures the history. + + Note: + Only the desired joint positions are used as inputs to the network. + """ + + cfg: ActuatorNetLSTMCfg + """The configuration of the actuator model.""" + + def __init__(self, cfg: ActuatorNetLSTMCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + + # load the model from JIT file + file_bytes = read_file(self.cfg.network_file) + self.network = torch.jit.load(file_bytes, map_location=self._device).eval() + + # extract number of lstm layers and hidden dim from the shape of weights + num_layers = len(self.network.lstm.state_dict()) // 4 + hidden_dim = self.network.lstm.state_dict()["weight_hh_l0"].shape[1] + # create buffers for storing LSTM inputs + self.sea_input = torch.zeros(self._num_envs * self.num_joints, 1, 2, device=self._device) + self.sea_hidden_state = torch.zeros( + num_layers, self._num_envs * self.num_joints, hidden_dim, device=self._device + ) + self.sea_cell_state = torch.zeros(num_layers, self._num_envs * self.num_joints, hidden_dim, device=self._device) + # reshape via views (doesn't change the actual memory layout) + layer_shape_per_env = (num_layers, self._num_envs, self.num_joints, hidden_dim) + self.sea_hidden_state_per_env = self.sea_hidden_state.view(layer_shape_per_env) + self.sea_cell_state_per_env = self.sea_cell_state.view(layer_shape_per_env) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int]): + # reset the hidden and cell states for the specified environments + with torch.no_grad(): + self.sea_hidden_state_per_env[:, env_ids] = 0.0 + self.sea_cell_state_per_env[:, env_ids] = 0.0 + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # compute network inputs + self.sea_input[:, 0, 0] = (control_action.joint_positions - joint_pos).flatten() + self.sea_input[:, 0, 1] = joint_vel.flatten() + # save current joint vel for dc-motor clipping + self._joint_vel[:] = joint_vel + + # run network inference + with torch.inference_mode(): + torques, (self.sea_hidden_state[:], self.sea_cell_state[:]) = self.network( + self.sea_input, (self.sea_hidden_state, self.sea_cell_state) + ) + self.computed_effort = torques.reshape(self._num_envs, self.num_joints) + + # clip the computed effort based on the motor limits + self.applied_effort = self._clip_effort(self.computed_effort) + + # return torques + control_action.joint_efforts = self.applied_effort + control_action.joint_positions = None + control_action.joint_velocities = None + return control_action + + +class ActuatorNetMLP(DCMotor): + """Actuator model based on multi-layer perceptron and joint history. + + Many times the analytical model is not sufficient to capture the actuator dynamics, the + delay in the actuator response, or the non-linearities in the actuator. In these cases, + a neural network model can be used to approximate the actuator dynamics. This model is + trained using data collected from the physical actuator and maps the joint state and the + desired joint command to the produced torque by the actuator. + + This class implements the learned model as a neural network based on the work from + :cite:t:`hwangbo2019learning`. The class stores the history of the joint positions errors + and velocities which are used to provide input to the neural network. The model is loaded + as a TorchScript. + + Note: + Only the desired joint positions are used as inputs to the network. + + """ + + cfg: ActuatorNetMLPCfg + """The configuration of the actuator model.""" + + def __init__(self, cfg: ActuatorNetMLPCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + + # load the model from JIT file + file_bytes = read_file(self.cfg.network_file) + self.network = torch.jit.load(file_bytes, map_location=self._device).eval() + + # create buffers for MLP history + history_length = max(self.cfg.input_idx) + 1 + self._joint_pos_error_history = torch.zeros( + self._num_envs, history_length, self.num_joints, device=self._device + ) + self._joint_vel_history = torch.zeros(self._num_envs, history_length, self.num_joints, device=self._device) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int]): + # reset the history for the specified environments + self._joint_pos_error_history[env_ids] = 0.0 + self._joint_vel_history[env_ids] = 0.0 + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # move history queue by 1 and update top of history + # -- positions + self._joint_pos_error_history = self._joint_pos_error_history.roll(1, 1) + self._joint_pos_error_history[:, 0] = control_action.joint_positions - joint_pos + # -- velocity + self._joint_vel_history = self._joint_vel_history.roll(1, 1) + self._joint_vel_history[:, 0] = joint_vel + # save current joint vel for dc-motor clipping + self._joint_vel[:] = joint_vel + + # compute network inputs + # -- positions + pos_input = torch.cat([self._joint_pos_error_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) + pos_input = pos_input.view(self._num_envs * self.num_joints, -1) + # -- velocity + vel_input = torch.cat([self._joint_vel_history[:, i].unsqueeze(2) for i in self.cfg.input_idx], dim=2) + vel_input = vel_input.view(self._num_envs * self.num_joints, -1) + # -- scale and concatenate inputs + if self.cfg.input_order == "pos_vel": + network_input = torch.cat([pos_input * self.cfg.pos_scale, vel_input * self.cfg.vel_scale], dim=1) + elif self.cfg.input_order == "vel_pos": + network_input = torch.cat([vel_input * self.cfg.vel_scale, pos_input * self.cfg.pos_scale], dim=1) + else: + raise ValueError( + f"Invalid input order for MLP actuator net: {self.cfg.input_order}. Must be 'pos_vel' or 'vel_pos'." + ) + + # run network inference + with torch.inference_mode(): + torques = self.network(network_input).view(self._num_envs, self.num_joints) + self.computed_effort = torques.view(self._num_envs, self.num_joints) * self.cfg.torque_scale + + # clip the computed effort based on the motor limits + self.applied_effort = self._clip_effort(self.computed_effort) + + # return torques + control_action.joint_efforts = self.applied_effort + control_action.joint_positions = None + control_action.joint_velocities = None + return control_action diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_pd.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_pd.py new file mode 100644 index 00000000000..e76b06191d1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/actuators/actuator_pd.py @@ -0,0 +1,421 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +from isaaclab.utils import DelayBuffer, LinearInterpolation +from isaaclab.utils.types import ArticulationActions + +from .actuator_base import ActuatorBase + +if TYPE_CHECKING: + from .actuator_cfg import ( + DCMotorCfg, + DelayedPDActuatorCfg, + IdealPDActuatorCfg, + ImplicitActuatorCfg, + RemotizedPDActuatorCfg, + ) + + +""" +Implicit Actuator Models. +""" + + +class ImplicitActuator(ActuatorBase): + """Implicit actuator model that is handled by the simulation. + + This performs a similar function as the :class:`IdealPDActuator` class. However, the PD control is handled + implicitly by the simulation which performs continuous-time integration of the PD control law. This is + generally more accurate than the explicit PD control law used in :class:`IdealPDActuator` when the simulation + time-step is large. + + The articulation class sets the stiffness and damping parameters from the implicit actuator configuration + into the simulation. Thus, the class does not perform its own computations on the joint action that + needs to be applied to the simulation. However, it computes the approximate torques for the actuated joint + since PhysX does not expose this quantity explicitly. + + .. caution:: + + The class is only provided for consistency with the other actuator models. It does not implement any + functionality and should not be used. All values should be set to the simulation directly. + """ + + cfg: ImplicitActuatorCfg + """The configuration for the actuator model.""" + + def __init__(self, cfg: ImplicitActuatorCfg, *args, **kwargs): + # effort limits + if cfg.effort_limit_sim is None and cfg.effort_limit is not None: + # throw a warning that we have a replacement for the deprecated parameter + omni.log.warn( + "The object has a value for 'effort_limit'." + " This parameter will be removed in the future." + " To set the effort limit, please use 'effort_limit_sim' instead." + ) + cfg.effort_limit_sim = cfg.effort_limit + elif cfg.effort_limit_sim is not None and cfg.effort_limit is None: + # TODO: Eventually we want to get rid of 'effort_limit' for implicit actuators. + # We should do this once all parameters have an "_sim" suffix. + cfg.effort_limit = cfg.effort_limit_sim + elif cfg.effort_limit_sim is not None and cfg.effort_limit is not None: + if cfg.effort_limit_sim != cfg.effort_limit: + raise ValueError( + "The object has set both 'effort_limit_sim' and 'effort_limit'" + f" and they have different values {cfg.effort_limit_sim} != {cfg.effort_limit}." + " Please only set 'effort_limit_sim' for implicit actuators." + ) + + # velocity limits + if cfg.velocity_limit_sim is None and cfg.velocity_limit is not None: + # throw a warning that previously this was not set + # it leads to different simulation behavior so we want to remain backwards compatible + omni.log.warn( + "The object has a value for 'velocity_limit'." + " Previously, although this value was specified, it was not getting used by implicit" + " actuators. Since this parameter affects the simulation behavior, we continue to not" + " use it. This parameter will be removed in the future." + " To set the velocity limit, please use 'velocity_limit_sim' instead." + ) + cfg.velocity_limit = None + elif cfg.velocity_limit_sim is not None and cfg.velocity_limit is None: + # TODO: Eventually we want to get rid of 'velocity_limit' for implicit actuators. + # We should do this once all parameters have an "_sim" suffix. + cfg.velocity_limit = cfg.velocity_limit_sim + elif cfg.velocity_limit_sim is not None and cfg.velocity_limit is not None: + if cfg.velocity_limit_sim != cfg.velocity_limit: + raise ValueError( + "The object has set both 'velocity_limit_sim' and 'velocity_limit'" + f" and they have different values {cfg.velocity_limit_sim} != {cfg.velocity_limit}." + " Please only set 'velocity_limit_sim' for implicit actuators." + ) + + # set implicit actuator model flag + ImplicitActuator.is_implicit_model = True + # call the base class + super().__init__(cfg, *args, **kwargs) + + """ + Operations. + """ + + def reset(self, *args, **kwargs): + # This is a no-op. There is no state to reset for implicit actuators. + pass + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + """Process the actuator group actions and compute the articulation actions. + + In case of implicit actuator, the control action is directly returned as the computed action. + This function is a no-op and does not perform any computation on the input control action. + However, it computes the approximate torques for the actuated joint since PhysX does not compute + this quantity explicitly. + + Args: + control_action: The joint action instance comprising of the desired joint positions, joint velocities + and (feed-forward) joint efforts. + joint_pos: The current joint positions of the joints in the group. Shape is (num_envs, num_joints). + joint_vel: The current joint velocities of the joints in the group. Shape is (num_envs, num_joints). + + Returns: + The computed desired joint positions, joint velocities and joint efforts. + """ + # store approximate torques for reward computation + error_pos = control_action.joint_positions - joint_pos + error_vel = control_action.joint_velocities - joint_vel + self.computed_effort = self.stiffness * error_pos + self.damping * error_vel + control_action.joint_efforts + # clip the torques based on the motor limits + self.applied_effort = self._clip_effort(self.computed_effort) + return control_action + + +""" +Explicit Actuator Models. +""" + + +class IdealPDActuator(ActuatorBase): + r"""Ideal torque-controlled actuator model with a simple saturation model. + + It employs the following model for computing torques for the actuated joint :math:`j`: + + .. math:: + + \tau_{j, computed} = k_p * (q - q_{des}) + k_d * (\dot{q} - \dot{q}_{des}) + \tau_{ff} + + where, :math:`k_p` and :math:`k_d` are joint stiffness and damping gains, :math:`q` and :math:`\dot{q}` + are the current joint positions and velocities, :math:`q_{des}`, :math:`\dot{q}_{des}` and :math:`\tau_{ff}` + are the desired joint positions, velocities and torques commands. + + The clipping model is based on the maximum torque applied by the motor. It is implemented as: + + .. math:: + + \tau_{j, max} & = \gamma \times \tau_{motor, max} \\ + \tau_{j, applied} & = clip(\tau_{computed}, -\tau_{j, max}, \tau_{j, max}) + + where the clipping function is defined as :math:`clip(x, x_{min}, x_{max}) = min(max(x, x_{min}), x_{max})`. + The parameters :math:`\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends, + and :math:`\tau_{motor, max}` is the maximum motor effort possible. These parameters are read from + the configuration instance passed to the class. + """ + + cfg: IdealPDActuatorCfg + """The configuration for the actuator model.""" + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int]): + pass + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # compute errors + error_pos = control_action.joint_positions - joint_pos + error_vel = control_action.joint_velocities - joint_vel + # calculate the desired joint torques + self.computed_effort = self.stiffness * error_pos + self.damping * error_vel + control_action.joint_efforts + # clip the torques based on the motor limits + self.applied_effort = self._clip_effort(self.computed_effort) + # set the computed actions back into the control action + control_action.joint_efforts = self.applied_effort + control_action.joint_positions = None + control_action.joint_velocities = None + return control_action + + +class DCMotor(IdealPDActuator): + r"""Direct control (DC) motor actuator model with velocity-based saturation model. + + It uses the same model as the :class:`IdealActuator` for computing the torques from input commands. + However, it implements a saturation model defined by DC motor characteristics. + + A DC motor is a type of electric motor that is powered by direct current electricity. In most cases, + the motor is connected to a constant source of voltage supply, and the current is controlled by a rheostat. + Depending on various design factors such as windings and materials, the motor can draw a limited maximum power + from the electronic source, which limits the produced motor torque and speed. + + A DC motor characteristics are defined by the following parameters: + + * Continuous-rated speed (:math:`\dot{q}_{motor, max}`) : The maximum-rated speed of the motor. + * Continuous-stall torque (:math:`\tau_{motor, max}`): The maximum-rated torque produced at 0 speed. + * Saturation torque (:math:`\tau_{motor, sat}`): The maximum torque that can be outputted for a short period. + + Based on these parameters, the instantaneous minimum and maximum torques are defined as follows: + + .. math:: + + \tau_{j, max}(\dot{q}) & = clip \left (\tau_{j, sat} \times \left(1 - + \frac{\dot{q}}{\dot{q}_{j, max}}\right), 0.0, \tau_{j, max} \right) \\ + \tau_{j, min}(\dot{q}) & = clip \left (\tau_{j, sat} \times \left( -1 - + \frac{\dot{q}}{\dot{q}_{j, max}}\right), - \tau_{j, max}, 0.0 \right) + + where :math:`\gamma` is the gear ratio of the gear box connecting the motor and the actuated joint ends, + :math:`\dot{q}_{j, max} = \gamma^{-1} \times \dot{q}_{motor, max}`, :math:`\tau_{j, max} = + \gamma \times \tau_{motor, max}` and :math:`\tau_{j, peak} = \gamma \times \tau_{motor, peak}` + are the maximum joint velocity, maximum joint torque and peak torque, respectively. These parameters + are read from the configuration instance passed to the class. + + Using these values, the computed torques are clipped to the minimum and maximum values based on the + instantaneous joint velocity: + + .. math:: + + \tau_{j, applied} = clip(\tau_{computed}, \tau_{j, min}(\dot{q}), \tau_{j, max}(\dot{q})) + + """ + + cfg: DCMotorCfg + """The configuration for the actuator model.""" + + def __init__(self, cfg: DCMotorCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + # parse configuration + if self.cfg.saturation_effort is not None: + self._saturation_effort = self.cfg.saturation_effort + else: + self._saturation_effort = torch.inf + # prepare joint vel buffer for max effort computation + self._joint_vel = torch.zeros_like(self.computed_effort) + # create buffer for zeros effort + self._zeros_effort = torch.zeros_like(self.computed_effort) + # check that quantities are provided + if self.cfg.velocity_limit is None: + raise ValueError("The velocity limit must be provided for the DC motor actuator model.") + + """ + Operations. + """ + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # save current joint vel + self._joint_vel[:] = joint_vel + # calculate the desired joint torques + return super().compute(control_action, joint_pos, joint_vel) + + """ + Helper functions. + """ + + def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: + # compute torque limits + # -- max limit + max_effort = self._saturation_effort * (1.0 - self._joint_vel / self.velocity_limit) + max_effort = torch.clip(max_effort, min=self._zeros_effort, max=self.effort_limit) + # -- min limit + min_effort = self._saturation_effort * (-1.0 - self._joint_vel / self.velocity_limit) + min_effort = torch.clip(min_effort, min=-self.effort_limit, max=self._zeros_effort) + + # clip the torques based on the motor limits + return torch.clip(effort, min=min_effort, max=max_effort) + + +class DelayedPDActuator(IdealPDActuator): + """Ideal PD actuator with delayed command application. + + This class extends the :class:`IdealPDActuator` class by adding a delay to the actuator commands. The delay + is implemented using a circular buffer that stores the actuator commands for a certain number of physics steps. + The most recent actuation value is pushed to the buffer at every physics step, but the final actuation value + applied to the simulation is lagged by a certain number of physics steps. + + The amount of time lag is configurable and can be set to a random value between the minimum and maximum time + lag bounds at every reset. The minimum and maximum time lag values are set in the configuration instance passed + to the class. + """ + + cfg: DelayedPDActuatorCfg + """The configuration for the actuator model.""" + + def __init__(self, cfg: DelayedPDActuatorCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + # instantiate the delay buffers + self.positions_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + self.velocities_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + self.efforts_delay_buffer = DelayBuffer(cfg.max_delay, self._num_envs, device=self._device) + # all of the envs + self._ALL_INDICES = torch.arange(self._num_envs, dtype=torch.long, device=self._device) + + def reset(self, env_ids: Sequence[int]): + super().reset(env_ids) + # number of environments (since env_ids can be a slice) + if env_ids is None or env_ids == slice(None): + num_envs = self._num_envs + else: + num_envs = len(env_ids) + # set a new random delay for environments in env_ids + time_lags = torch.randint( + low=self.cfg.min_delay, + high=self.cfg.max_delay + 1, + size=(num_envs,), + dtype=torch.int, + device=self._device, + ) + # set delays + self.positions_delay_buffer.set_time_lag(time_lags, env_ids) + self.velocities_delay_buffer.set_time_lag(time_lags, env_ids) + self.efforts_delay_buffer.set_time_lag(time_lags, env_ids) + # reset buffers + self.positions_delay_buffer.reset(env_ids) + self.velocities_delay_buffer.reset(env_ids) + self.efforts_delay_buffer.reset(env_ids) + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # apply delay based on the delay the model for all the setpoints + control_action.joint_positions = self.positions_delay_buffer.compute(control_action.joint_positions) + control_action.joint_velocities = self.velocities_delay_buffer.compute(control_action.joint_velocities) + control_action.joint_efforts = self.efforts_delay_buffer.compute(control_action.joint_efforts) + # compte actuator model + return super().compute(control_action, joint_pos, joint_vel) + + +class RemotizedPDActuator(DelayedPDActuator): + """Ideal PD actuator with angle-dependent torque limits. + + This class extends the :class:`DelayedPDActuator` class by adding angle-dependent torque limits to the actuator. + The torque limits are applied by querying a lookup table describing the relationship between the joint angle + and the maximum output torque. The lookup table is provided in the configuration instance passed to the class. + + The torque limits are interpolated based on the current joint positions and applied to the actuator commands. + """ + + def __init__( + self, + cfg: RemotizedPDActuatorCfg, + joint_names: list[str], + joint_ids: Sequence[int], + num_envs: int, + device: str, + stiffness: torch.Tensor | float = 0.0, + damping: torch.Tensor | float = 0.0, + armature: torch.Tensor | float = 0.0, + friction: torch.Tensor | float = 0.0, + effort_limit: torch.Tensor | float = torch.inf, + velocity_limit: torch.Tensor | float = torch.inf, + ): + # remove effort and velocity box constraints from the base class + cfg.effort_limit = torch.inf + cfg.velocity_limit = torch.inf + # call the base method and set default effort_limit and velocity_limit to inf + super().__init__( + cfg, joint_names, joint_ids, num_envs, device, stiffness, damping, armature, friction, torch.inf, torch.inf + ) + self._joint_parameter_lookup = torch.tensor(cfg.joint_parameter_lookup, device=device) + # define remotized joint torque limit + self._torque_limit = LinearInterpolation(self.angle_samples, self.max_torque_samples, device=device) + + """ + Properties. + """ + + @property + def angle_samples(self) -> torch.Tensor: + return self._joint_parameter_lookup[:, 0] + + @property + def transmission_ratio_samples(self) -> torch.Tensor: + return self._joint_parameter_lookup[:, 1] + + @property + def max_torque_samples(self) -> torch.Tensor: + return self._joint_parameter_lookup[:, 2] + + """ + Operations. + """ + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # call the base method + control_action = super().compute(control_action, joint_pos, joint_vel) + # compute the absolute torque limits for the current joint positions + abs_torque_limits = self._torque_limit.compute(joint_pos) + # apply the limits + control_action.joint_efforts = torch.clamp( + control_action.joint_efforts, min=-abs_torque_limits, max=abs_torque_limits + ) + self.applied_effort = control_action.joint_efforts + return control_action diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/__init__.py new file mode 100644 index 00000000000..db0c695b742 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/__init__.py @@ -0,0 +1,20 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package containing app-specific functionalities. + +These include: + +* Ability to launch the simulation app with different configurations +* Run tests with the simulation app + +""" + +from .app_launcher import AppLauncher # noqa: F401, F403 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/app_launcher.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/app_launcher.py new file mode 100644 index 00000000000..1d48c0dd671 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/app/app_launcher.py @@ -0,0 +1,1014 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package with the utility class to configure the :class:`isaacsim.simulation_app.SimulationApp`. + +The :class:`AppLauncher` parses environment variables and input CLI arguments to launch the simulator in +various different modes. This includes with or without GUI and switching between different Omniverse remote +clients. Some of these require the extensions to be loaded in a specific order, otherwise a segmentation +fault occurs. The launched :class:`isaacsim.simulation_app.SimulationApp` instance is accessible via the +:attr:`AppLauncher.app` property. +""" + +import argparse +import contextlib +import os +import re +import signal +import sys +import toml +import warnings +from typing import Any, Literal + +import flatdict + +with contextlib.suppress(ModuleNotFoundError): + import isaacsim # noqa: F401 + +from isaacsim import SimulationApp + + +class ExplicitAction(argparse.Action): + """Custom action to track if an argument was explicitly passed by the user.""" + + def __call__(self, parser, namespace, values, option_string=None): + # Set the parameter value + setattr(namespace, self.dest, values) + # Set a flag indicating the parameter was explicitly passed + setattr(namespace, f"{self.dest}_explicit", True) + + +class AppLauncher: + """A utility class to launch Isaac Sim application based on command-line arguments and environment variables. + + The class resolves the simulation app settings that appear through environments variables, + command-line arguments (CLI) or as input keyword arguments. Based on these settings, it launches the + simulation app and configures the extensions to load (as a part of post-launch setup). + + The input arguments provided to the class are given higher priority than the values set + from the corresponding environment variables. This provides flexibility to deal with different + users' preferences. + + .. note:: + Explicitly defined arguments are only given priority when their value is set to something outside + their default configuration. For example, the ``livestream`` argument is -1 by default. It only + overrides the ``LIVESTREAM`` environment variable when ``livestream`` argument is set to a + value >-1. In other words, if ``livestream=-1``, then the value from the environment variable + ``LIVESTREAM`` is used. + + """ + + def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwargs): + """Create a `SimulationApp`_ instance based on the input settings. + + Args: + launcher_args: Input arguments to parse using the AppLauncher and set into the SimulationApp. + Defaults to None, which is equivalent to passing an empty dictionary. A detailed description of + the possible arguments is available in the `SimulationApp`_ documentation. + **kwargs : Additional keyword arguments that will be merged into :attr:`launcher_args`. + They serve as a convenience for those who want to pass some arguments using the argparse + interface and others directly into the AppLauncher. Duplicated arguments with + the :attr:`launcher_args` will raise a ValueError. + + Raises: + ValueError: If there are common/duplicated arguments between ``launcher_args`` and ``kwargs``. + ValueError: If combination of ``launcher_args`` and ``kwargs`` are missing the necessary arguments + that are needed by the AppLauncher to resolve the desired app configuration. + ValueError: If incompatible or undefined values are assigned to relevant environment values, + such as ``LIVESTREAM``. + + .. _argparse.Namespace: https://docs.python.org/3/library/argparse.html?highlight=namespace#argparse.Namespace + .. _SimulationApp: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.simulation_app/docs/index.html + """ + # We allow users to pass either a dict or an argparse.Namespace into + # __init__, anticipating that these will be all of the argparse arguments + # used by the calling script. Those which we appended via add_app_launcher_args + # will be used to control extension loading logic. Additional arguments are allowed, + # and will be passed directly to the SimulationApp initialization. + # + # We could potentially require users to enter each argument they want passed here + # as a kwarg, but this would require them to pass livestream, headless, and + # any other options we choose to add here explicitly, and with the correct keywords. + # + # @hunter: I feel that this is cumbersome and could introduce error, and would prefer to do + # some sanity checking in the add_app_launcher_args function + if launcher_args is None: + launcher_args = {} + elif isinstance(launcher_args, argparse.Namespace): + launcher_args = launcher_args.__dict__ + + # Check that arguments are unique + if len(kwargs) > 0: + if not set(kwargs.keys()).isdisjoint(launcher_args.keys()): + overlapping_args = set(kwargs.keys()).intersection(launcher_args.keys()) + raise ValueError( + f"Input `launcher_args` and `kwargs` both provided common attributes: {overlapping_args}." + " Please ensure that each argument is supplied to only one of them, as the AppLauncher cannot" + " discern priority between them." + ) + launcher_args.update(kwargs) + + # Define config members that are read from env-vars or keyword args + self._headless: bool # 0: GUI, 1: Headless + self._livestream: Literal[0, 1, 2] # 0: Disabled, 1: Native, 2: WebRTC + self._offscreen_render: bool # 0: Disabled, 1: Enabled + self._sim_experience_file: str # Experience file to load + + # Exposed to train scripts + self.device_id: int # device ID for GPU simulation (defaults to 0) + self.local_rank: int # local rank of GPUs in the current node + self.global_rank: int # global rank for multi-node training + + # Integrate env-vars and input keyword args into simulation app config + self._config_resolution(launcher_args) + + # Internal: Override SimulationApp._start_app method to apply patches after app has started. + self.__patch_simulation_start_app(launcher_args) + + # Create SimulationApp, passing the resolved self._config to it for initialization + self._create_app() + # Load IsaacSim extensions + self._load_extensions() + # Hide the stop button in the toolbar + self._hide_stop_button() + # Set settings from the given rendering mode + self._set_rendering_mode_settings(launcher_args) + + # Hide play button callback if the timeline is stopped + import omni.timeline + + self._hide_play_button_callback = ( + omni.timeline.get_timeline_interface() + .get_timeline_event_stream() + .create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), lambda e: self._hide_play_button(True) + ) + ) + self._unhide_play_button_callback = ( + omni.timeline.get_timeline_interface() + .get_timeline_event_stream() + .create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda e: self._hide_play_button(False) + ) + ) + # Set up signal handlers for graceful shutdown + # -- during explicit `kill` commands + signal.signal(signal.SIGTERM, self._abort_signal_handle_callback) + # -- during segfaults + signal.signal(signal.SIGABRT, self._abort_signal_handle_callback) + signal.signal(signal.SIGSEGV, self._abort_signal_handle_callback) + + """ + Properties. + """ + + @property + def app(self) -> SimulationApp: + """The launched SimulationApp.""" + if self._app is not None: + return self._app + else: + raise RuntimeError("The `AppLauncher.app` member cannot be retrieved until the class is initialized.") + + """ + Operations. + """ + + @staticmethod + def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: + """Utility function to configure AppLauncher arguments with an existing argument parser object. + + This function takes an ``argparse.ArgumentParser`` object and does some sanity checking on the existing + arguments for ingestion by the SimulationApp. It then appends custom command-line arguments relevant + to the SimulationApp to the input :class:`argparse.ArgumentParser` instance. This allows overriding the + environment variables using command-line arguments. + + Currently, it adds the following parameters to the argparser object: + + * ``headless`` (bool): If True, the app will be launched in headless (no-gui) mode. The values map the same + as that for the ``HEADLESS`` environment variable. If False, then headless mode is determined by the + ``HEADLESS`` environment variable. + * ``livestream`` (int): If one of {1, 2}, then livestreaming and headless mode is enabled. The values + map the same as that for the ``LIVESTREAM`` environment variable. If :obj:`-1`, then livestreaming is + determined by the ``LIVESTREAM`` environment variable. + Valid options are: + + - ``0``: Disabled + - ``1``: `Native [DEPRECATED] `_ + - ``2``: `WebRTC `_ + + * ``enable_cameras`` (bool): If True, the app will enable camera sensors and render them, even when in + headless mode. This flag must be set to True if the environments contains any camera sensors. + The values map the same as that for the ``ENABLE_CAMERAS`` environment variable. + If False, then enable_cameras mode is determined by the ``ENABLE_CAMERAS`` environment variable. + * ``device`` (str): The device to run the simulation on. + Valid options are: + + - ``cpu``: Use CPU. + - ``cuda``: Use GPU with device ID ``0``. + - ``cuda:N``: Use GPU, where N is the device ID. For example, "cuda:0". + + * ``experience`` (str): The experience file to load when launching the SimulationApp. If a relative path + is provided, it is resolved relative to the ``apps`` folder in Isaac Sim and Isaac Lab (in that order). + + If provided as an empty string, the experience file is determined based on the command-line flags: + + * If headless and enable_cameras are True, the experience file is set to ``isaaclab.python.headless.rendering.kit``. + * If headless is False and enable_cameras is True, the experience file is set to ``isaaclab.python.rendering.kit``. + * If headless and enable_cameras are False, the experience file is set to ``isaaclab.python.kit``. + * If headless is True and enable_cameras is False, the experience file is set to ``isaaclab.python.headless.kit``. + + * ``kit_args`` (str): Optional command line arguments to be passed to Omniverse Kit directly. + Arguments should be combined into a single string separated by space. + Example usage: --kit_args "--ext-folder=/path/to/ext1 --ext-folder=/path/to/ext2" + + Args: + parser: An argument parser instance to be extended with the AppLauncher specific options. + """ + # If the passed parser has an existing _HelpAction when passed, + # we here remove the options which would invoke it, + # to be added back after the additional AppLauncher args + # have been added. This is equivalent to + # initially constructing the ArgParser with add_help=False, + # but this means we don't have to require that behavior + # in users and can handle it on our end. + # We do this because calling parse_known_args() will handle + # any -h/--help options being passed and then exit immediately, + # before the additional arguments can be added to the help readout. + parser_help = None + if len(parser._actions) > 0 and isinstance(parser._actions[0], argparse._HelpAction): # type: ignore + parser_help = parser._actions[0] + parser._option_string_actions.pop("-h") + parser._option_string_actions.pop("--help") + + # Parse known args for potential name collisions/type mismatches + # between the config fields SimulationApp expects and the ArgParse + # arguments that the user passed. + known, _ = parser.parse_known_args() + config = vars(known) + if len(config) == 0: + print( + "[WARN][AppLauncher]: There are no arguments attached to the ArgumentParser object." + " If you have your own arguments, please load your own arguments before calling the" + " `AppLauncher.add_app_launcher_args` method. This allows the method to check the validity" + " of the arguments and perform checks for argument names." + ) + else: + AppLauncher._check_argparser_config_params(config) + + # Add custom arguments to the parser + arg_group = parser.add_argument_group( + "app_launcher arguments", + description="Arguments for the AppLauncher. For more details, please check the documentation.", + ) + arg_group.add_argument( + "--headless", + action="store_true", + default=AppLauncher._APPLAUNCHER_CFG_INFO["headless"][1], + help="Force display off at all times.", + ) + arg_group.add_argument( + "--livestream", + type=int, + default=AppLauncher._APPLAUNCHER_CFG_INFO["livestream"][1], + choices={0, 1, 2}, + help="Force enable livestreaming. Mapping corresponds to that for the `LIVESTREAM` environment variable.", + ) + arg_group.add_argument( + "--enable_cameras", + action="store_true", + default=AppLauncher._APPLAUNCHER_CFG_INFO["enable_cameras"][1], + help="Enable camera sensors and relevant extension dependencies.", + ) + arg_group.add_argument( + "--xr", + action="store_true", + default=AppLauncher._APPLAUNCHER_CFG_INFO["xr"][1], + help="Enable XR mode for VR/AR applications.", + ) + arg_group.add_argument( + "--device", + type=str, + action=ExplicitAction, + default=AppLauncher._APPLAUNCHER_CFG_INFO["device"][1], + help='The device to run the simulation on. Can be "cpu", "cuda", "cuda:N", where N is the device ID', + ) + # Add the deprecated cpu flag to raise an error if it is used + arg_group.add_argument("--cpu", action="store_true", help=argparse.SUPPRESS) + arg_group.add_argument( + "--verbose", # Note: This is read by SimulationApp through sys.argv + action="store_true", + help="Enable verbose-level log output from the SimulationApp.", + ) + arg_group.add_argument( + "--info", # Note: This is read by SimulationApp through sys.argv + action="store_true", + help="Enable info-level log output from the SimulationApp.", + ) + arg_group.add_argument( + "--experience", + type=str, + default="", + help=( + "The experience file to load when launching the SimulationApp. If an empty string is provided," + " the experience file is determined based on the headless flag. If a relative path is provided," + " it is resolved relative to the `apps` folder in Isaac Sim and Isaac Lab (in that order)." + ), + ) + arg_group.add_argument( + "--rendering_mode", + type=str, + action=ExplicitAction, + choices={"performance", "balanced", "quality", "xr"}, + help=( + "Sets the rendering mode. Preset settings files can be found in apps/rendering_modes." + ' Can be "performance", "balanced", "quality", or "xr".' + " Individual settings can be overwritten by using the RenderCfg class." + ), + ) + arg_group.add_argument( + "--kit_args", + type=str, + default="", + help=( + "Command line arguments for Omniverse Kit as a string separated by a space delimiter." + ' Example usage: --kit_args "--ext-folder=/path/to/ext1 --ext-folder=/path/to/ext2"' + ), + ) + + # Corresponding to the beginning of the function, + # if we have removed -h/--help handling, we add it back. + if parser_help is not None: + parser._option_string_actions["-h"] = parser_help + parser._option_string_actions["--help"] = parser_help + + """ + Internal functions. + """ + + _APPLAUNCHER_CFG_INFO: dict[str, tuple[list[type], Any]] = { + "headless": ([bool], False), + "livestream": ([int], -1), + "enable_cameras": ([bool], False), + "xr": ([bool], False), + "device": ([str], "cuda:0"), + "experience": ([str], ""), + "rendering_mode": ([str], "balanced"), + } + """A dictionary of arguments added manually by the :meth:`AppLauncher.add_app_launcher_args` method. + + The values are a tuple of the expected type and default value. This is used to check against name collisions + for arguments passed to the :class:`AppLauncher` class as well as for type checking. + + They have corresponding environment variables as detailed in the documentation. + """ + + # TODO: Find some internally managed NVIDIA list of these types. + # SimulationApp.DEFAULT_LAUNCHER_CONFIG almost works, except that + # it is ambiguous where the default types are None + _SIM_APP_CFG_TYPES: dict[str, list[type]] = { + "headless": [bool], + "hide_ui": [bool, type(None)], + "active_gpu": [int, type(None)], + "physics_gpu": [int], + "multi_gpu": [bool], + "sync_loads": [bool], + "width": [int], + "height": [int], + "window_width": [int], + "window_height": [int], + "display_options": [int], + "subdiv_refinement_level": [int], + "renderer": [str], + "anti_aliasing": [int], + "samples_per_pixel_per_frame": [int], + "denoiser": [bool], + "max_bounces": [int], + "max_specular_transmission_bounces": [int], + "max_volume_bounces": [int], + "open_usd": [str, type(None)], + "livesync_usd": [str, type(None)], + "fast_shutdown": [bool], + "experience": [str], + } + """A dictionary containing the type of arguments passed to SimulationApp. + + This is used to check against name collisions for arguments passed to the :class:`AppLauncher` class + as well as for type checking. It corresponds closely to the :attr:`SimulationApp.DEFAULT_LAUNCHER_CONFIG`, + but specifically denotes where None types are allowed. + """ + + @staticmethod + def _check_argparser_config_params(config: dict) -> None: + """Checks that input argparser object has parameters with valid settings with no name conflicts. + + First, we inspect the dictionary to ensure that the passed ArgParser object is not attempting to add arguments + which should be assigned by calling :meth:`AppLauncher.add_app_launcher_args`. + + Then, we check that if the key corresponds to a config setting expected by SimulationApp, then the type of + that key's value corresponds to the type expected by the SimulationApp. If it passes the check, the function + prints out that the setting with be passed to the SimulationApp. Otherwise, we raise a ValueError exception. + + Args: + config: A configuration parameters which will be passed to the SimulationApp constructor. + + Raises: + ValueError: If a key is an already existing field in the configuration parameters but + should be added by calling the :meth:`AppLauncher.add_app_launcher_args. + ValueError: If keys corresponding to those used to initialize SimulationApp + (as found in :attr:`_SIM_APP_CFG_TYPES`) are of the wrong value type. + """ + # check that no config key conflicts with AppLauncher config names + applauncher_keys = set(AppLauncher._APPLAUNCHER_CFG_INFO.keys()) + for key, value in config.items(): + if key in applauncher_keys: + raise ValueError( + f"The passed ArgParser object already has the field '{key}'. This field will be added by" + " `AppLauncher.add_app_launcher_args()`, and should not be added directly. Please remove the" + " argument or rename it to a non-conflicting name." + ) + # check that type of the passed keys are valid + simulationapp_keys = set(AppLauncher._SIM_APP_CFG_TYPES.keys()) + for key, value in config.items(): + if key in simulationapp_keys: + given_type = type(value) + expected_types = AppLauncher._SIM_APP_CFG_TYPES[key] + if type(value) not in set(expected_types): + raise ValueError( + f"Invalid value type for the argument '{key}': {given_type}. Expected one of {expected_types}," + " if intended to be ingested by the SimulationApp object. Please change the type if this" + " intended for the SimulationApp or change the name of the argument to avoid name conflicts." + ) + # Print out values which will be used + print(f"[INFO][AppLauncher]: The argument '{key}' will be used to configure the SimulationApp.") + + def _config_resolution(self, launcher_args: dict): + """Resolve the input arguments and environment variables. + + Args: + launcher_args: A dictionary of all input arguments passed to the class object. + """ + # Handle core settings + livestream_arg, livestream_env = self._resolve_livestream_settings(launcher_args) + self._resolve_headless_settings(launcher_args, livestream_arg, livestream_env) + self._resolve_camera_settings(launcher_args) + self._resolve_xr_settings(launcher_args) + self._resolve_viewport_settings(launcher_args) + + # Handle device and distributed settings + self._resolve_device_settings(launcher_args) + + # Handle experience file settings + self._resolve_experience_file(launcher_args) + + # Handle additional arguments + self._resolve_kit_args(launcher_args) + + # Prepare final simulation app config + # Remove all values from input keyword args which are not meant for SimulationApp + # Assign all the passed settings to a dictionary for the simulation app + self._sim_app_config = { + key: launcher_args[key] for key in set(AppLauncher._SIM_APP_CFG_TYPES.keys()) & set(launcher_args.keys()) + } + + def _resolve_livestream_settings(self, launcher_args: dict) -> tuple[int, int]: + """Resolve livestream related settings.""" + livestream_env = int(os.environ.get("LIVESTREAM", 0)) + livestream_arg = launcher_args.pop("livestream", AppLauncher._APPLAUNCHER_CFG_INFO["livestream"][1]) + livestream_valid_vals = {0, 1, 2} + # Value checking on LIVESTREAM + if livestream_env not in livestream_valid_vals: + raise ValueError( + f"Invalid value for environment variable `LIVESTREAM`: {livestream_env} ." + f" Expected: {livestream_valid_vals}." + ) + # We allow livestream kwarg to supersede LIVESTREAM envvar + if livestream_arg >= 0: + if livestream_arg in livestream_valid_vals: + self._livestream = livestream_arg + # print info that we overrode the env-var + print( + f"[INFO][AppLauncher]: Input keyword argument `livestream={livestream_arg}` has overridden" + f" the environment variable `LIVESTREAM={livestream_env}`." + ) + else: + raise ValueError( + f"Invalid value for input keyword argument `livestream`: {livestream_arg} ." + f" Expected: {livestream_valid_vals}." + ) + else: + self._livestream = livestream_env + + # Process livestream here before launching kit because some of the extensions only work when launched with the kit file + self._livestream_args = [] + if self._livestream >= 1: + # Note: Only one livestream extension can be enabled at a time + if self._livestream == 1: + warnings.warn( + "Native Livestream is deprecated. Please use WebRTC Livestream instead with --livestream 2." + ) + self._livestream_args += [ + '--/app/livestream/proto="ws"', + "--/app/livestream/allowResize=true", + "--enable", + "omni.kit.livestream.core-4.1.2", + "--enable", + "omni.kit.livestream.native-5.0.1", + "--enable", + "omni.kit.streamsdk.plugins-4.1.1", + ] + elif self._livestream == 2: + self._livestream_args += [ + "--/app/livestream/allowResize=false", + "--enable", + "omni.kit.livestream.webrtc", + ] + else: + raise ValueError(f"Invalid value for livestream: {self._livestream}. Expected: 1, 2 .") + sys.argv += self._livestream_args + + return livestream_arg, livestream_env + + def _resolve_headless_settings(self, launcher_args: dict, livestream_arg: int, livestream_env: int): + """Resolve headless related settings.""" + # Resolve headless execution of simulation app + # HEADLESS is initially passed as an int instead of + # the bool of headless_arg to avoid messy string processing, + headless_env = int(os.environ.get("HEADLESS", 0)) + headless_arg = launcher_args.pop("headless", AppLauncher._APPLAUNCHER_CFG_INFO["headless"][1]) + headless_valid_vals = {0, 1} + # Value checking on HEADLESS + if headless_env not in headless_valid_vals: + raise ValueError( + f"Invalid value for environment variable `HEADLESS`: {headless_env} . Expected: {headless_valid_vals}." + ) + # We allow headless kwarg to supersede HEADLESS envvar if headless_arg does not have the default value + # Note: Headless is always true when livestreaming + if headless_arg is True: + self._headless = headless_arg + elif self._livestream in {1, 2}: + # we are always headless on the host machine + self._headless = True + # inform who has toggled the headless flag + if self._livestream == livestream_arg: + print( + f"[INFO][AppLauncher]: Input keyword argument `livestream={self._livestream}` has implicitly" + f" overridden the environment variable `HEADLESS={headless_env}` to True." + ) + elif self._livestream == livestream_env: + print( + f"[INFO][AppLauncher]: Environment variable `LIVESTREAM={self._livestream}` has implicitly" + f" overridden the environment variable `HEADLESS={headless_env}` to True." + ) + else: + # Headless needs to be a bool to be ingested by SimulationApp + self._headless = bool(headless_env) + # Headless needs to be passed to the SimulationApp so we keep it here + launcher_args["headless"] = self._headless + + def _resolve_camera_settings(self, launcher_args: dict): + """Resolve camera related settings.""" + enable_cameras_env = int(os.environ.get("ENABLE_CAMERAS", 0)) + enable_cameras_arg = launcher_args.pop("enable_cameras", AppLauncher._APPLAUNCHER_CFG_INFO["enable_cameras"][1]) + enable_cameras_valid_vals = {0, 1} + if enable_cameras_env not in enable_cameras_valid_vals: + raise ValueError( + f"Invalid value for environment variable `ENABLE_CAMERAS`: {enable_cameras_env} ." + f"Expected: {enable_cameras_valid_vals} ." + ) + # We allow enable_cameras kwarg to supersede ENABLE_CAMERAS envvar + if enable_cameras_arg is True: + self._enable_cameras = enable_cameras_arg + else: + self._enable_cameras = bool(enable_cameras_env) + self._offscreen_render = False + if self._enable_cameras and self._headless: + self._offscreen_render = True + + def _resolve_xr_settings(self, launcher_args: dict): + """Resolve XR related settings.""" + xr_env = int(os.environ.get("XR", 0)) + xr_arg = launcher_args.pop("xr", AppLauncher._APPLAUNCHER_CFG_INFO["xr"][1]) + xr_valid_vals = {0, 1} + if xr_env not in xr_valid_vals: + raise ValueError(f"Invalid value for environment variable `XR`: {xr_env} .Expected: {xr_valid_vals} .") + # We allow xr kwarg to supersede XR envvar + if xr_arg is True: + self._xr = xr_arg + else: + self._xr = bool(xr_env) + + def _resolve_viewport_settings(self, launcher_args: dict): + """Resolve viewport related settings.""" + # Check if we can disable the viewport to improve performance + # This should only happen if we are running headless and do not require livestreaming or video recording + # This is different from offscreen_render because this only affects the default viewport and not other renderproducts in the scene + self._render_viewport = True + if self._headless and not self._livestream and not launcher_args.get("video", False): + self._render_viewport = False + + # hide_ui flag + launcher_args["hide_ui"] = False + if self._headless and not self._livestream: + launcher_args["hide_ui"] = True + + # avoid creating new stage at startup by default for performance reasons + launcher_args["create_new_stage"] = False + + def _resolve_device_settings(self, launcher_args: dict): + """Resolve simulation GPU device related settings.""" + self.device_id = 0 + device = launcher_args.get("device", AppLauncher._APPLAUNCHER_CFG_INFO["device"][1]) + + device_explicitly_passed = launcher_args.pop("device_explicit", False) + if self._xr and not device_explicitly_passed: + # If no device is specified, default to the CPU device if we are running in XR + device = "cpu" + + # Overwrite for downstream consumers + launcher_args["device"] = "cpu" + + if "cuda" not in device and "cpu" not in device: + raise ValueError( + f"Invalid value for input keyword argument `device`: {device}." + " Expected: a string with the format 'cuda', 'cuda:', or 'cpu'." + ) + + if "cuda:" in device: + self.device_id = int(device.split(":")[-1]) + + # Raise an error for the deprecated cpu flag + if launcher_args.get("cpu", False): + raise ValueError("The `--cpu` flag is deprecated. Please use `--device cpu` instead.") + + if "distributed" in launcher_args and launcher_args["distributed"]: + # local rank (GPU id) in a current multi-gpu mode + self.local_rank = int(os.getenv("LOCAL_RANK", "0")) + int(os.getenv("JAX_LOCAL_RANK", "0")) + # global rank (GPU id) in multi-gpu multi-node mode + self.global_rank = int(os.getenv("RANK", "0")) + int(os.getenv("JAX_RANK", "0")) + + self.device_id = self.local_rank + launcher_args["multi_gpu"] = False + # limit CPU threads to minimize thread context switching + # this ensures processes do not take up all available threads and fight for resources + num_cpu_cores = os.cpu_count() + num_threads_per_process = num_cpu_cores // int(os.getenv("WORLD_SIZE", 1)) + # set environment variables to limit CPU threads + os.environ["PXR_WORK_THREAD_LIMIT"] = str(num_threads_per_process) + os.environ["OPENBLAS_NUM_THREADS"] = str(num_threads_per_process) + # pass command line variable to kit + sys.argv.append(f"--/plugins/carb.tasking.plugin/threadCount={num_threads_per_process}") + + # set physics and rendering device + launcher_args["physics_gpu"] = self.device_id + launcher_args["active_gpu"] = self.device_id + + print(f"[INFO][AppLauncher]: Using device: {device}") + + def _resolve_experience_file(self, launcher_args: dict): + """Resolve experience file related settings.""" + # Check if input keywords contain an 'experience' file setting + # Note: since experience is taken as a separate argument by Simulation App, we store it separately + self._sim_experience_file = launcher_args.pop("experience", "") + + # If nothing is provided resolve the experience file based on the headless flag + kit_app_exp_path = os.environ["EXP_PATH"] + isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") + if self._sim_experience_file == "": + # check if the headless flag is set + if self._enable_cameras: + if self._headless and not self._livestream: + self._sim_experience_file = os.path.join( + isaaclab_app_exp_path, "isaaclab.python.headless.rendering.kit" + ) + else: + self._sim_experience_file = os.path.join(isaaclab_app_exp_path, "isaaclab.python.rendering.kit") + elif self._xr: + if self._headless: + self._sim_experience_file = os.path.join( + isaaclab_app_exp_path, "isaaclab.python.xr.openxr.headless.kit" + ) + else: + self._sim_experience_file = os.path.join(isaaclab_app_exp_path, "isaaclab.python.xr.openxr.kit") + elif self._headless and not self._livestream: + self._sim_experience_file = os.path.join(isaaclab_app_exp_path, "isaaclab.python.headless.kit") + else: + self._sim_experience_file = os.path.join(isaaclab_app_exp_path, "isaaclab.python.kit") + elif not os.path.isabs(self._sim_experience_file): + option_1_app_exp_path = os.path.join(kit_app_exp_path, self._sim_experience_file) + option_2_app_exp_path = os.path.join(isaaclab_app_exp_path, self._sim_experience_file) + if os.path.exists(option_1_app_exp_path): + self._sim_experience_file = option_1_app_exp_path + elif os.path.exists(option_2_app_exp_path): + self._sim_experience_file = option_2_app_exp_path + else: + raise FileNotFoundError( + f"Invalid value for input keyword argument `experience`: {self._sim_experience_file}." + "\n No such file exists in either the Kit or Isaac Lab experience paths. Checked paths:" + f"\n\t [1]: {option_1_app_exp_path}" + f"\n\t [2]: {option_2_app_exp_path}" + ) + elif not os.path.exists(self._sim_experience_file): + raise FileNotFoundError( + f"Invalid value for input keyword argument `experience`: {self._sim_experience_file}." + " The file does not exist." + ) + + # Set public IP address of a remote instance + public_ip_env = os.environ.get("PUBLIC_IP", "127.0.0.1") + + # Process livestream here before launching kit because some of the extensions only work when launched with the kit file + self._livestream_args = [] + if self._livestream >= 1: + # Note: Only one livestream extension can be enabled at a time + if self._livestream == 1: + warnings.warn( + "Native Livestream is deprecated. Please use WebRTC Livestream instead with --livestream 2." + ) + self._livestream_args += [ + '--/app/livestream/proto="ws"', + "--/app/livestream/allowResize=true", + "--enable", + "omni.kit.livestream.core-4.1.2", + "--enable", + "omni.kit.livestream.native-5.0.1", + "--enable", + "omni.kit.streamsdk.plugins-4.1.1", + ] + elif self._livestream == 2: + self._livestream_args += [ + f"--/app/livestream/publicEndpointAddress={public_ip_env}", + "--/app/livestream/port=49100", + "--enable", + "omni.services.livestream.nvcf", + ] + else: + raise ValueError(f"Invalid value for livestream: {self._livestream}. Expected: 1, 2 .") + sys.argv += self._livestream_args + # Resolve the absolute path of the experience file + self._sim_experience_file = os.path.abspath(self._sim_experience_file) + print(f"[INFO][AppLauncher]: Loading experience file: {self._sim_experience_file}") + + def _resolve_kit_args(self, launcher_args: dict): + """Resolve additional arguments passed to Kit.""" + # Resolve additional arguments passed to Kit + self._kit_args = [] + if "kit_args" in launcher_args: + self._kit_args = [arg for arg in launcher_args["kit_args"].split()] + sys.argv += self._kit_args + + def _create_app(self): + """Launch and create the SimulationApp based on the parsed simulation config.""" + # Initialize SimulationApp + # hack sys module to make sure that the SimulationApp is initialized correctly + # this is to avoid the warnings from the simulation app about not ok modules + r = re.compile(".*lab.*") + found_modules = list(filter(r.match, list(sys.modules.keys()))) + # remove Isaac Lab modules from sys.modules + hacked_modules = dict() + for key in found_modules: + hacked_modules[key] = sys.modules[key] + del sys.modules[key] + + # disable sys stdout and stderr to avoid printing the warning messages + # this is mainly done to purge the print statements from the simulation app + if "--verbose" not in sys.argv and "--info" not in sys.argv: + sys.stdout = open(os.devnull, "w") # noqa: SIM115 + # launch simulation app + self._app = SimulationApp(self._sim_app_config, experience=self._sim_experience_file) + # enable sys stdout and stderr + sys.stdout = sys.__stdout__ + + # add Isaac Lab modules back to sys.modules + for key, value in hacked_modules.items(): + sys.modules[key] = value + # remove the threadCount argument from sys.argv if it was added for distributed training + pattern = r"--/plugins/carb\.tasking\.plugin/threadCount=\d+" + sys.argv = [arg for arg in sys.argv if not re.match(pattern, arg)] + # remove additional OV args from sys.argv + if len(self._kit_args) > 0: + sys.argv = [arg for arg in sys.argv if arg not in self._kit_args] + if len(self._livestream_args) > 0: + sys.argv = [arg for arg in sys.argv if arg not in self._livestream_args] + + def _rendering_enabled(self) -> bool: + """Check if rendering is required by the app.""" + # Indicates whether rendering is required by the app. + # Extensions required for rendering bring startup and simulation costs, so we do not enable them if not required. + return not self._headless or self._livestream >= 1 or self._enable_cameras or self._xr + + def _load_extensions(self): + """Load correct extensions based on AppLauncher's resolved config member variables.""" + # These have to be loaded after SimulationApp is initialized + import carb + import omni.physx.bindings._physx as physx_impl + + # Retrieve carb settings for modification + carb_settings_iface = carb.settings.get_settings() + + # set carb setting to indicate Isaac Lab's offscreen_render pipeline should be enabled + # this flag is used by the SimulationContext class to enable the offscreen_render pipeline + # when the render() method is called. + carb_settings_iface.set_bool("/isaaclab/render/offscreen", self._offscreen_render) + + # set carb setting to indicate Isaac Lab's render_viewport pipeline should be enabled + # this flag is used by the SimulationContext class to enable the render_viewport pipeline + # when the render() method is called. + carb_settings_iface.set_bool("/isaaclab/render/active_viewport", self._render_viewport) + + # set carb setting to indicate no RTX sensors are used + # this flag is set to True when an RTX-rendering related sensor is created + # for example: the `Camera` sensor class + carb_settings_iface.set_bool("/isaaclab/render/rtx_sensors", False) + + # set fabric update flag to disable updating transforms when rendering is disabled + carb_settings_iface.set_bool("/physics/fabricUpdateTransformations", self._rendering_enabled()) + + # disable physics backwards compatibility check + carb_settings_iface.set_int(physx_impl.SETTING_BACKWARD_COMPATIBILITY, 0) + + def _hide_stop_button(self): + """Hide the stop button in the toolbar. + + For standalone executions, having a stop button is confusing since it invalidates the whole simulation. + Thus, we hide the button so that users don't accidentally click it. + """ + # when we are truly headless, then we can't import the widget toolbar + # thus, we only hide the stop button when we are not headless (i.e. GUI is enabled) + if self._livestream >= 1 or not self._headless: + import omni.kit.widget.toolbar + + # grey out the stop button because we don't want to stop the simulation manually in standalone mode + toolbar = omni.kit.widget.toolbar.get_instance() + play_button_group = toolbar._builtin_tools._play_button_group # type: ignore + if play_button_group is not None: + play_button_group._stop_button.visible = False # type: ignore + play_button_group._stop_button.enabled = False # type: ignore + play_button_group._stop_button = None # type: ignore + + def _set_rendering_mode_settings(self, launcher_args: dict) -> None: + """Set RTX rendering settings to the values from the selected preset.""" + import carb + from isaacsim.core.utils.carb import set_carb_setting + + rendering_mode = launcher_args.get("rendering_mode") + + # use default kit rendering settings if cameras are disabled and a rendering mode is not selected + if not self._enable_cameras and rendering_mode is None: + return + + # default to balanced mode + if rendering_mode is None: + rendering_mode = "balanced" + + rendering_mode_explicitly_passed = launcher_args.pop("rendering_mode_explicit", False) + if self._xr and not rendering_mode_explicitly_passed: + # If no rendering mode is specified, default to the xr mode if we are running in XR + rendering_mode = "xr" + + # Overwrite for downstream consumers + launcher_args["rendering_mode"] = "xr" + + # parse preset file + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + preset_filename = os.path.join(repo_path, f"apps/rendering_modes/{rendering_mode}.kit") + with open(preset_filename) as file: + preset_dict = toml.load(file) + preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) + + # set presets + carb_setting = carb.settings.get_settings() + for key, value in preset_dict.items(): + key = "/" + key.replace(".", "/") # convert to carb setting format + set_carb_setting(carb_setting, key, value) + + def _interrupt_signal_handle_callback(self, signal, frame): + """Handle the interrupt signal from the keyboard.""" + # close the app + self._app.close() + # raise the error for keyboard interrupt + raise KeyboardInterrupt + + def _hide_play_button(self, flag): + """Hide/Unhide the play button in the toolbar. + + This is used if the timeline is stopped by a GUI action like "save as" to not allow the user to + resume the timeline afterwards. + """ + # when we are truly headless, then we can't import the widget toolbar + # thus, we only hide the play button when we are not headless (i.e. GUI is enabled) + if self._livestream >= 1 or not self._headless: + import omni.kit.widget.toolbar + + toolbar = omni.kit.widget.toolbar.get_instance() + play_button_group = toolbar._builtin_tools._play_button_group # type: ignore + if play_button_group is not None: + play_button_group._play_button.visible = not flag # type: ignore + play_button_group._play_button.enabled = not flag # type: ignore + + def _abort_signal_handle_callback(self, signal, frame): + """Handle the abort/segmentation/kill signals.""" + # close the app + self._app.close() + + def __patch_simulation_start_app(self, launcher_args: dict): + if not launcher_args.get("enable_pinocchio", False): + return + + if launcher_args.get("disable_pinocchio_patch", False): + return + + original_start_app = SimulationApp._start_app + + def _start_app_patch(sim_app_instance, *args, **kwargs): + original_start_app(sim_app_instance, *args, **kwargs) + self.__patch_pxr_gf_matrix4d(launcher_args) + + SimulationApp._start_app = _start_app_patch + + def __patch_pxr_gf_matrix4d(self, launcher_args: dict): + import traceback + + import carb + from pxr import Gf + + carb.log_warn( + "Due to an issue with Pinocchio and pxr.Gf.Matrix4d, patching the Matrix4d constructor to convert arguments" + " into a list of floats." + ) + + # Store the original Matrix4d constructor + original_matrix4d = Gf.Matrix4d.__init__ + + # Define a wrapper function to handle different input types + def patch_matrix4d(self, *args, **kwargs): + try: + # Case 1: No arguments (identity matrix) + if len(args) == 0: + original_matrix4d(self, *args, **kwargs) + return + + # Case 2: Single argument + elif len(args) == 1: + arg = args[0] + + # Case 2a: Already a Matrix4d + if isinstance(arg, Gf.Matrix4d): + original_matrix4d(self, arg) + return + + # Case 2b: Tuple of tuples (4x4 matrix) OR List of lists (4x4 matrix) + elif (isinstance(arg, tuple) and len(arg) == 4 and all(isinstance(row, tuple) for row in arg)) or ( + isinstance(arg, list) and len(arg) == 4 and all(isinstance(row, list) for row in arg) + ): + float_list = [float(item) for row in arg for item in row] + original_matrix4d(self, *float_list) + return + + # Case 2c: Flat list of 16 elements + elif isinstance(arg, (list, tuple)) and len(arg) == 16: + float_list = [float(item) for item in arg] + original_matrix4d(self, *float_list) + return + + # Case 2d: Another matrix-like object with elements accessible via indexing + elif hasattr(arg, "__getitem__") and hasattr(arg, "__len__"): + with contextlib.suppress(IndexError, TypeError): + if len(arg) == 16: + float_list = [float(arg[i]) for i in range(16)] + original_matrix4d(self, *float_list) + return + # Try to extract as 4x4 matrix + elif len(arg) == 4 and all(len(row) == 4 for row in arg): + float_list = [float(arg[i][j]) for i in range(4) for j in range(4)] + original_matrix4d(self, *float_list) + return + + # Case 3: 16 separate arguments (individual matrix elements) + elif len(args) == 16: + float_list = [float(arg) for arg in args] + original_matrix4d(self, *float_list) + return + + # Default: Use original constructor + original_matrix4d(self, *args, **kwargs) + + except Exception as e: + carb.log_error(f"Matrix4d wrapper error: {e}") + traceback.print_stack() + # Fall back to original constructor as last resort + try: + original_matrix4d(self, *args, **kwargs) + except Exception as inner_e: + carb.log_error(f"Original Matrix4d constructor also failed: {inner_e}") + # Initialize as identity matrix if all else fails + original_matrix4d(self) + + Gf.Matrix4d.__init__ = patch_matrix4d diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/__init__.py new file mode 100644 index 00000000000..376b80b1499 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/__init__.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for different assets, such as rigid objects and articulations. + +An asset is a physical object that can be spawned in the simulation. The class handles both +the spawning of the asset into the USD stage as well as initialization of necessary physics +handles to interact with the asset. + +Upon construction of the asset instance, the prim corresponding to the asset is spawned into the +USD stage if the spawn configuration is not None. The spawn configuration is defined in the +:attr:`AssetBaseCfg.spawn` attribute. In case the configured :attr:`AssetBaseCfg.prim_path` is +an expression, then the prim is spawned at all the matching paths. Otherwise, a single prim is +spawned at the configured path. For more information on the spawn configuration, see the +:mod:`isaaclab.sim.spawners` module. + +The asset class also registers callbacks for the stage play/stop events. These are used to +construct the physics handles for the asset as the physics engine is only available when the +stage is playing. Additionally, the class registers a callback for debug visualization of the +asset. This can be enabled by setting the :attr:`AssetBaseCfg.debug_vis` attribute to True. + +The asset class follows the following naming convention for its methods: + +* **set_xxx()**: These are used to only set the buffers into the :attr:`data` instance. However, they + do not write the data into the simulator. The writing of data only happens when the + :meth:`write_data_to_sim` method is called. +* **write_xxx_to_sim()**: These are used to set the buffers into the :attr:`data` instance and write + the corresponding data into the simulator as well. +* **update(dt)**: These are used to update the buffers in the :attr:`data` instance. This should + be called after a simulation step is performed. + +The main reason to separate the ``set`` and ``write`` operations is to provide flexibility to the +user when they need to perform a post-processing operation of the buffers before applying them +into the simulator. A common example for this is dealing with explicit actuator models where the +specified joint targets are not directly applied to the simulator but are instead used to compute +the corresponding actuator torques. +""" + +from .articulation import Articulation, ArticulationCfg, ArticulationData +from .asset_base import AssetBase +from .asset_base_cfg import AssetBaseCfg +from .deformable_object import DeformableObject, DeformableObjectCfg, DeformableObjectData +from .rigid_object import RigidObject, RigidObjectCfg, RigidObjectData +from .rigid_object_collection import RigidObjectCollection, RigidObjectCollectionCfg, RigidObjectCollectionData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/__init__.py new file mode 100644 index 00000000000..e3d88e21dbf --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for rigid articulated assets.""" + +from .articulation import Articulation +from .articulation_cfg import ArticulationCfg +from .articulation_data import ArticulationData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation.py new file mode 100644 index 00000000000..7fb6846a10a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation.py @@ -0,0 +1,1677 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Flag for pyright to ignore type errors in this file. +# pyright: reportPrivateUsage=false + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +import isaacsim.core.utils.stage as stage_utils +import omni.log +import omni.physics.tensors.impl.api as physx +from isaacsim.core.simulation_manager import SimulationManager +from pxr import PhysxSchema, UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils +from isaaclab.actuators import ActuatorBase, ActuatorBaseCfg, ImplicitActuator +from isaaclab.utils.types import ArticulationActions + +from ..asset_base import AssetBase +from .articulation_data import ArticulationData + +if TYPE_CHECKING: + from .articulation_cfg import ArticulationCfg + + +class Articulation(AssetBase): + """An articulation asset class. + + An articulation is a collection of rigid bodies connected by joints. The joints can be either + fixed or actuated. The joints can be of different types, such as revolute, prismatic, D-6, etc. + However, the articulation class has currently been tested with revolute and prismatic joints. + The class supports both floating-base and fixed-base articulations. The type of articulation + is determined based on the root joint of the articulation. If the root joint is fixed, then + the articulation is considered a fixed-base system. Otherwise, it is considered a floating-base + system. This can be checked using the :attr:`Articulation.is_fixed_base` attribute. + + For an asset to be considered an articulation, the root prim of the asset must have the + `USD ArticulationRootAPI`_. This API is used to define the sub-tree of the articulation using + the reduced coordinate formulation. On playing the simulation, the physics engine parses the + articulation root prim and creates the corresponding articulation in the physics engine. The + articulation root prim can be specified using the :attr:`AssetBaseCfg.prim_path` attribute. + + The articulation class also provides the functionality to augment the simulation of an articulated + system with custom actuator models. These models can either be explicit or implicit, as detailed in + the :mod:`isaaclab.actuators` module. The actuator models are specified using the + :attr:`ArticulationCfg.actuators` attribute. These are then parsed and used to initialize the + corresponding actuator models, when the simulation is played. + + During the simulation step, the articulation class first applies the actuator models to compute + the joint commands based on the user-specified targets. These joint commands are then applied + into the simulation. The joint commands can be either position, velocity, or effort commands. + As an example, the following snippet shows how this can be used for position commands: + + .. code-block:: python + + # an example instance of the articulation class + my_articulation = Articulation(cfg) + + # set joint position targets + my_articulation.set_joint_position_target(position) + # propagate the actuator models and apply the computed commands into the simulation + my_articulation.write_data_to_sim() + + # step the simulation using the simulation context + sim_context.step() + + # update the articulation state, where dt is the simulation time step + my_articulation.update(dt) + + .. _`USD ArticulationRootAPI`: https://openusd.org/dev/api/class_usd_physics_articulation_root_a_p_i.html + + """ + + cfg: ArticulationCfg + """Configuration instance for the articulations.""" + + actuators: dict[str, ActuatorBase] + """Dictionary of actuator instances for the articulation. + + The keys are the actuator names and the values are the actuator instances. The actuator instances + are initialized based on the actuator configurations specified in the :attr:`ArticulationCfg.actuators` + attribute. They are used to compute the joint commands during the :meth:`write_data_to_sim` function. + """ + + def __init__(self, cfg: ArticulationCfg): + """Initialize the articulation. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + def data(self) -> ArticulationData: + return self._data + + @property + def num_instances(self) -> int: + return self.root_physx_view.count + + @property + def is_fixed_base(self) -> bool: + """Whether the articulation is a fixed-base or floating-base system.""" + return self.root_physx_view.shared_metatype.fixed_base + + @property + def num_joints(self) -> int: + """Number of joints in articulation.""" + return self.root_physx_view.shared_metatype.dof_count + + @property + def num_fixed_tendons(self) -> int: + """Number of fixed tendons in articulation.""" + return self.root_physx_view.max_fixed_tendons + + @property + def num_bodies(self) -> int: + """Number of bodies in articulation.""" + return self.root_physx_view.shared_metatype.link_count + + @property + def joint_names(self) -> list[str]: + """Ordered names of joints in articulation.""" + return self.root_physx_view.shared_metatype.dof_names + + @property + def fixed_tendon_names(self) -> list[str]: + """Ordered names of fixed tendons in articulation.""" + return self._fixed_tendon_names + + @property + def body_names(self) -> list[str]: + """Ordered names of bodies in articulation.""" + return self.root_physx_view.shared_metatype.link_names + + @property + def root_physx_view(self) -> physx.ArticulationView: + """Articulation view for the asset (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._root_physx_view + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # use ellipses object to skip initial indices. + if env_ids is None: + env_ids = slice(None) + # reset actuators + for actuator in self.actuators.values(): + actuator.reset(env_ids) + # reset external wrench + self._external_force_b[env_ids] = 0.0 + self._external_torque_b[env_ids] = 0.0 + + def write_data_to_sim(self): + """Write external wrenches and joint commands to the simulation. + + If any explicit actuators are present, then the actuator models are used to compute the + joint commands. Otherwise, the joint commands are directly set into the simulation. + + Note: + We write external wrench to the simulation here since this function is called before the simulation step. + This ensures that the external wrench is applied at every simulation step. + """ + # write external wrench + if self.has_external_wrench: + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._external_force_b.view(-1, 3), + torque_data=self._external_torque_b.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + + # apply actuator models + self._apply_actuator_model() + # write actions into simulation + self.root_physx_view.set_dof_actuation_forces(self._joint_effort_target_sim, self._ALL_INDICES) + # position and velocity targets only for implicit actuators + if self._has_implicit_actuators: + self.root_physx_view.set_dof_position_targets(self._joint_pos_target_sim, self._ALL_INDICES) + self.root_physx_view.set_dof_velocity_targets(self._joint_vel_target_sim, self._ALL_INDICES) + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the articulation based on the name keys. + + Please check the :meth:`isaaclab.utils.string_utils.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self.body_names, preserve_order) + + def find_joints( + self, name_keys: str | Sequence[str], joint_subset: list[str] | None = None, preserve_order: bool = False + ) -> tuple[list[int], list[str]]: + """Find joints in the articulation based on the name keys. + + Please see the :func:`isaaclab.utils.string.resolve_matching_names` function for more information + on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the joint names. + joint_subset: A subset of joints to search for. Defaults to None, which means all joints + in the articulation are searched. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the joint indices and names. + """ + if joint_subset is None: + joint_subset = self.joint_names + # find joints + return string_utils.resolve_matching_names(name_keys, joint_subset, preserve_order) + + def find_fixed_tendons( + self, name_keys: str | Sequence[str], tendon_subsets: list[str] | None = None, preserve_order: bool = False + ) -> tuple[list[int], list[str]]: + """Find fixed tendons in the articulation based on the name keys. + + Please see the :func:`isaaclab.utils.string.resolve_matching_names` function for more information + on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the joint + names with fixed tendons. + tendon_subsets: A subset of joints with fixed tendons to search for. Defaults to None, which means + all joints in the articulation are searched. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the tendon indices and names. + """ + if tendon_subsets is None: + # tendons follow the joint names they are attached to + tendon_subsets = self.fixed_tendon_names + # find tendons + return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order) + + """ + Operations - State Writers. + """ + + def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_com_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_link_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_link_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_pose, env_ids=env_ids) + + def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_link_pose_w[env_ids] = root_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_link_state_w.data is not None: + self._data.root_link_state_w[env_ids, :7] = self._data.root_link_pose_w[env_ids] + if self._data._root_state_w.data is not None: + self._data.root_state_w[env_ids, :7] = self._data.root_link_pose_w[env_ids] + + # convert root quaternion from wxyz to xyzw + root_poses_xyzw = self._data.root_link_pose_w.clone() + root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") + + # Need to invalidate the buffer to trigger the update with the new state. + self._data._body_link_pose_w.timestamp = -1.0 + self._data._body_com_pose_w.timestamp = -1.0 + self._data._body_state_w.timestamp = -1.0 + self._data._body_link_state_w.timestamp = -1.0 + self._data._body_com_state_w.timestamp = -1.0 + + # set into simulation + self.root_physx_view.set_root_transforms(root_poses_xyzw, indices=physx_env_ids) + + def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + The orientation is the orientation of the principle axes of inertia. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + if env_ids is None: + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids + + # set into internal buffers + self._data.root_com_pose_w[local_env_ids] = root_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_com_state_w.data is not None: + self._data.root_com_state_w[local_env_ids, :7] = self._data.root_com_pose_w[local_env_ids] + + # get CoM pose in link frame + com_pos_b = self.data.body_com_pos_b[local_env_ids, 0, :] + com_quat_b = self.data.body_com_quat_b[local_env_ids, 0, :] + # transform input CoM pose to link frame + root_link_pos, root_link_quat = math_utils.combine_frame_transforms( + root_pose[..., :3], + root_pose[..., 3:7], + math_utils.quat_apply(math_utils.quat_inv(com_quat_b), -com_pos_b), + math_utils.quat_inv(com_quat_b), + ) + root_link_pose = torch.cat((root_link_pos, root_link_quat), dim=-1) + + # write transformed pose in link frame to sim + self.write_root_link_pose_to_sim(root_pose=root_link_pose, env_ids=env_ids) + + def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's center of mass rather than the roots frame. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_com_velocity_to_sim(root_velocity=root_velocity, env_ids=env_ids) + + def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's center of mass rather than the roots frame. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_com_vel_w[env_ids] = root_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_com_state_w.data is not None: + self._data.root_com_state_w[env_ids, 7:] = self._data.root_com_vel_w[env_ids] + if self._data._root_state_w.data is not None: + self._data.root_state_w[env_ids, 7:] = self._data.root_com_vel_w[env_ids] + # make the acceleration zero to prevent reporting old values + self._data.body_acc_w[env_ids] = 0.0 + + # set into simulation + self.root_physx_view.set_root_velocities(self._data.root_com_vel_w, indices=physx_env_ids) + + def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's frame rather than the roots center of mass. + + Args: + root_velocity: Root frame velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + if env_ids is None: + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids + + # set into internal buffers + self._data.root_link_vel_w[local_env_ids] = root_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_link_state_w.data is not None: + self._data.root_link_state_w[local_env_ids, 7:] = self._data.root_link_vel_w[local_env_ids] + + # get CoM pose in link frame + quat = self.data.root_link_quat_w[local_env_ids] + com_pos_b = self.data.body_com_pos_b[local_env_ids, 0, :] + # transform input velocity to center of mass frame + root_com_velocity = root_velocity.clone() + root_com_velocity[:, :3] += torch.linalg.cross( + root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1 + ) + + # write transformed velocity in CoM frame to sim + self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=env_ids) + + def write_joint_state_to_sim( + self, + position: torch.Tensor, + velocity: torch.Tensor, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | slice | None = None, + ): + """Write joint positions and velocities to the simulation. + + Args: + position: Joint positions. Shape is (len(env_ids), len(joint_ids)). + velocity: Joint velocities. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # set into simulation + self.write_joint_position_to_sim(position, joint_ids=joint_ids, env_ids=env_ids) + self.write_joint_velocity_to_sim(velocity, joint_ids=joint_ids, env_ids=env_ids) + + def write_joint_position_to_sim( + self, + position: torch.Tensor, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | slice | None = None, + ): + """Write joint positions to the simulation. + + Args: + position: Joint positions. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_pos[env_ids, joint_ids] = position + # Need to invalidate the buffer to trigger the update with the new root pose. + self._data._body_state_w.timestamp = -1.0 + self._data._body_link_state_w.timestamp = -1.0 + self._data._body_com_state_w.timestamp = -1.0 + # set into simulation + self.root_physx_view.set_dof_positions(self._data.joint_pos, indices=physx_env_ids) + + def write_joint_velocity_to_sim( + self, + velocity: torch.Tensor, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | slice | None = None, + ): + """Write joint velocities to the simulation. + + Args: + velocity: Joint velocities. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_vel[env_ids, joint_ids] = velocity + self._data._previous_joint_vel[env_ids, joint_ids] = velocity + self._data.joint_acc[env_ids, joint_ids] = 0.0 + # set into simulation + self.root_physx_view.set_dof_velocities(self._data.joint_vel, indices=physx_env_ids) + + """ + Operations - Simulation Parameters Writers. + """ + + def write_joint_stiffness_to_sim( + self, + stiffness: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint stiffness into the simulation. + + Args: + stiffness: Joint stiffness. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the stiffness for. Defaults to None (all joints). + env_ids: The environment indices to set the stiffness for. Defaults to None (all environments). + """ + # note: This function isn't setting the values for actuator models. (#128) + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_stiffness[env_ids, joint_ids] = stiffness + # set into simulation + self.root_physx_view.set_dof_stiffnesses(self._data.joint_stiffness.cpu(), indices=physx_env_ids.cpu()) + + def write_joint_damping_to_sim( + self, + damping: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint damping into the simulation. + + Args: + damping: Joint damping. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the damping for. Defaults to None (all joints). + env_ids: The environment indices to set the damping for. Defaults to None (all environments). + """ + # note: This function isn't setting the values for actuator models. (#128) + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_damping[env_ids, joint_ids] = damping + # set into simulation + self.root_physx_view.set_dof_dampings(self._data.joint_damping.cpu(), indices=physx_env_ids.cpu()) + + def write_joint_position_limit_to_sim( + self, + limits: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + warn_limit_violation: bool = True, + ): + """Write joint position limits into the simulation. + + Args: + limits: Joint limits. Shape is (len(env_ids), len(joint_ids), 2). + joint_ids: The joint indices to set the limits for. Defaults to None (all joints). + env_ids: The environment indices to set the limits for. Defaults to None (all environments). + warn_limit_violation: Whether to use warning or info level logging when default joint positions + exceed the new limits. Defaults to True. + """ + # note: This function isn't setting the values for actuator models. (#128) + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_pos_limits[env_ids, joint_ids] = limits + # update default joint pos to stay within the new limits + if torch.any( + (self._data.default_joint_pos[env_ids, joint_ids] < limits[..., 0]) + | (self._data.default_joint_pos[env_ids, joint_ids] > limits[..., 1]) + ): + self._data.default_joint_pos[env_ids, joint_ids] = torch.clamp( + self._data.default_joint_pos[env_ids, joint_ids], limits[..., 0], limits[..., 1] + ) + violation_message = ( + "Some default joint positions are outside of the range of the new joint limits. Default joint positions" + " will be clamped to be within the new joint limits." + ) + if warn_limit_violation: + # warn level will show in console + omni.log.warn(violation_message) + else: + # info level is only written to log file + omni.log.info(violation_message) + # set into simulation + self.root_physx_view.set_dof_limits(self._data.joint_pos_limits.cpu(), indices=physx_env_ids.cpu()) + + # compute the soft limits based on the joint limits + # TODO: Optimize this computation for only selected joints + # soft joint position limits (recommended not to be too close to limits). + joint_pos_mean = (self._data.joint_pos_limits[..., 0] + self._data.joint_pos_limits[..., 1]) / 2 + joint_pos_range = self._data.joint_pos_limits[..., 1] - self._data.joint_pos_limits[..., 0] + soft_limit_factor = self.cfg.soft_joint_pos_limit_factor + # add to data + self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor + self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor + + def write_joint_velocity_limit_to_sim( + self, + limits: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint max velocity to the simulation. + + The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only + be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving + faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity. + + Args: + limits: Joint max velocity. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the max velocity for. Defaults to None (all joints). + env_ids: The environment indices to set the max velocity for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # move tensor to cpu if needed + if isinstance(limits, torch.Tensor): + limits = limits.to(self.device) + # set into internal buffers + self._data.joint_vel_limits[env_ids, joint_ids] = limits + # set into simulation + self.root_physx_view.set_dof_max_velocities(self._data.joint_vel_limits.cpu(), indices=physx_env_ids.cpu()) + + def write_joint_effort_limit_to_sim( + self, + limits: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint effort limits into the simulation. + + The effort limit is used to constrain the computed joint efforts in the physics engine. If the + computed effort exceeds this limit, the physics engine will clip the effort to this value. + + Args: + limits: Joint torque limits. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). + env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). + """ + # note: This function isn't setting the values for actuator models. (#128) + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # move tensor to cpu if needed + if isinstance(limits, torch.Tensor): + limits = limits.to(self.device) + # set into internal buffers + self._data.joint_effort_limits[env_ids, joint_ids] = limits + # set into simulation + self.root_physx_view.set_dof_max_forces(self._data.joint_effort_limits.cpu(), indices=physx_env_ids.cpu()) + + def write_joint_armature_to_sim( + self, + armature: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint armature into the simulation. + + The armature is directly added to the corresponding joint-space inertia. It helps improve the + simulation stability by reducing the joint velocities. + + Args: + armature: Joint armature. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). + env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_armature[env_ids, joint_ids] = armature + # set into simulation + self.root_physx_view.set_dof_armatures(self._data.joint_armature.cpu(), indices=physx_env_ids.cpu()) + + def write_joint_friction_coefficient_to_sim( + self, + joint_friction_coeff: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + r"""Write joint friction coefficients into the simulation. + + The joint friction is a unitless quantity. It relates the magnitude of the spatial force transmitted + from the parent body to the child body to the maximal friction force that may be applied by the solver + to resist the joint motion. + + Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` + is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force + transmitted from the parent body to the child body. The simulated friction effect is therefore + similar to static and Coulomb friction. + + Args: + joint_friction: Joint friction. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). + env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._data.joint_friction_coeff[env_ids, joint_ids] = joint_friction_coeff + # set into simulation + self.root_physx_view.set_dof_friction_coefficients( + self._data.joint_friction_coeff.cpu(), indices=physx_env_ids.cpu() + ) + + """ + Operations - Setters. + """ + + def set_external_force_and_torque( + self, + forces: torch.Tensor, + torques: torch.Tensor, + body_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set external force and torque to apply on the asset's bodies in their local frame. + + For many applications, we want to keep the applied external force on rigid bodies constant over a period of + time (for instance, during the policy control). This function allows us to store the external force and torque + into buffers which are then applied to the simulation at every step. + + .. caution:: + If the function is called with empty forces and torques, then this function disables the application + of external wrench to the simulation. + + .. code-block:: python + + # example of disabling external wrench + asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3)) + + .. note:: + This function does not apply the external wrench to the simulation. It only fills the buffers with + the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function + right before the simulation step. + + Args: + forces: External forces in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + torques: External torques in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + body_ids: Body indices to apply external wrench to. Defaults to None (all bodies). + env_ids: Environment indices to apply external wrench to. Defaults to None (all instances). + """ + if forces.any() or torques.any(): + self.has_external_wrench = True + else: + self.has_external_wrench = False + + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + elif not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device) + # -- body_ids + if body_ids is None: + body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device) + elif isinstance(body_ids, slice): + body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device)[body_ids] + elif not isinstance(body_ids, torch.Tensor): + body_ids = torch.tensor(body_ids, dtype=torch.long, device=self.device) + + # note: we need to do this complicated indexing since torch doesn't support multi-indexing + # create global body indices from env_ids and env_body_ids + # (env_id * total_bodies_per_env) + body_id + indices = body_ids.repeat(len(env_ids), 1) + env_ids.unsqueeze(1) * self.num_bodies + indices = indices.view(-1) + # set into internal buffers + # note: these are applied in the write_to_sim function + self._external_force_b.flatten(0, 1)[indices] = forces.flatten(0, 1) + self._external_torque_b.flatten(0, 1)[indices] = torques.flatten(0, 1) + + def set_joint_position_target( + self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None + ): + """Set joint position targets into internal buffers. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint position targets. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set targets + self._data.joint_pos_target[env_ids, joint_ids] = target + + def set_joint_velocity_target( + self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None + ): + """Set joint velocity targets into internal buffers. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint velocity targets. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set targets + self._data.joint_vel_target[env_ids, joint_ids] = target + + def set_joint_effort_target( + self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None + ): + """Set joint efforts into internal buffers. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint effort targets. Shape is (len(env_ids), len(joint_ids)). + joint_ids: The joint indices to set the targets for. Defaults to None (all joints). + env_ids: The environment indices to set the targets for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if joint_ids is None: + joint_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and joint_ids != slice(None): + env_ids = env_ids[:, None] + # set targets + self._data.joint_effort_target[env_ids, joint_ids] = target + + """ + Operations - Tendons. + """ + + def set_fixed_tendon_stiffness( + self, + stiffness: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon stiffness into internal buffers. + + This function does not apply the tendon stiffness to the simulation. It only fills the buffers with + the desired values. To apply the tendon stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + stiffness: Fixed tendon stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the stiffness for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the stiffness for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set stiffness + self._data.fixed_tendon_stiffness[env_ids, fixed_tendon_ids] = stiffness + + def set_fixed_tendon_damping( + self, + damping: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon damping into internal buffers. + + This function does not apply the tendon damping to the simulation. It only fills the buffers with + the desired values. To apply the tendon damping, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + damping: Fixed tendon damping. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the damping for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the damping for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set damping + self._data.fixed_tendon_damping[env_ids, fixed_tendon_ids] = damping + + def set_fixed_tendon_limit_stiffness( + self, + limit_stiffness: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon limit stiffness efforts into internal buffers. + + This function does not apply the tendon limit stiffness to the simulation. It only fills the buffers with + the desired values. To apply the tendon limit stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + limit_stiffness: Fixed tendon limit stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the limit stiffness for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the limit stiffness for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set limit_stiffness + self._data.fixed_tendon_limit_stiffness[env_ids, fixed_tendon_ids] = limit_stiffness + + def set_fixed_tendon_position_limit( + self, + limit: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon limit efforts into internal buffers. + + This function does not apply the tendon limit to the simulation. It only fills the buffers with + the desired values. To apply the tendon limit, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + limit: Fixed tendon limit. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the limit for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the limit for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set limit + self._data.fixed_tendon_pos_limits[env_ids, fixed_tendon_ids] = limit + + def set_fixed_tendon_rest_length( + self, + rest_length: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon rest length efforts into internal buffers. + + This function does not apply the tendon rest length to the simulation. It only fills the buffers with + the desired values. To apply the tendon rest length, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + rest_length: Fixed tendon rest length. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the rest length for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the rest length for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set rest_length + self._data.fixed_tendon_rest_length[env_ids, fixed_tendon_ids] = rest_length + + def set_fixed_tendon_offset( + self, + offset: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon offset efforts into internal buffers. + + This function does not apply the tendon offset to the simulation. It only fills the buffers with + the desired values. To apply the tendon offset, call the :meth:`write_fixed_tendon_properties_to_sim` function. + + Args: + offset: Fixed tendon offset. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the offset for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the offset for. Defaults to None (all environments). + """ + # resolve indices + if env_ids is None: + env_ids = slice(None) + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + if env_ids != slice(None) and fixed_tendon_ids != slice(None): + env_ids = env_ids[:, None] + # set offset + self._data.fixed_tendon_offset[env_ids, fixed_tendon_ids] = offset + + def write_fixed_tendon_properties_to_sim( + self, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write fixed tendon properties into the simulation. + + Args: + fixed_tendon_ids: The fixed tendon indices to set the limits for. Defaults to None (all fixed tendons). + env_ids: The environment indices to set the limits for. Defaults to None (all environments). + """ + # resolve indices + physx_env_ids = env_ids + if env_ids is None: + physx_env_ids = self._ALL_INDICES + if fixed_tendon_ids is None: + fixed_tendon_ids = slice(None) + + # set into simulation + self.root_physx_view.set_fixed_tendon_properties( + self._data.fixed_tendon_stiffness, + self._data.fixed_tendon_damping, + self._data.fixed_tendon_limit_stiffness, + self._data.fixed_tendon_pos_limits, + self._data.fixed_tendon_rest_length, + self._data.fixed_tendon_offset, + indices=physx_env_ids, + ) + + """ + Internal helper. + """ + + def _initialize_impl(self): + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + + if self.cfg.articulation_root_prim_path is not None: + # The articulation root prim path is specified explicitly, so we can just use this. + root_prim_path_expr = self.cfg.prim_path + self.cfg.articulation_root_prim_path + else: + # No articulation root prim path was specified, so we need to search + # for it. We search for this in the first environment and then + # create a regex that matches all environments. + first_env_matching_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if first_env_matching_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") + first_env_matching_prim_path = first_env_matching_prim.GetPath().pathString + + # Find all articulation root prims in the first environment. + first_env_root_prims = sim_utils.get_all_matching_child_prims( + first_env_matching_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), + ) + if len(first_env_root_prims) == 0: + raise RuntimeError( + f"Failed to find an articulation when resolving '{first_env_matching_prim_path}'." + " Please ensure that the prim has 'USD ArticulationRootAPI' applied." + ) + if len(first_env_root_prims) > 1: + raise RuntimeError( + f"Failed to find a single articulation when resolving '{first_env_matching_prim_path}'." + f" Found multiple '{first_env_root_prims}' under '{first_env_matching_prim_path}'." + " Please ensure that there is only one articulation in the prim path tree." + ) + + # Now we convert the found articulation root from the first + # environment back into a regex that matches all environments. + first_env_root_prim_path = first_env_root_prims[0].GetPath().pathString + root_prim_path_relative_to_prim_path = first_env_root_prim_path[len(first_env_matching_prim_path) :] + root_prim_path_expr = self.cfg.prim_path + root_prim_path_relative_to_prim_path + + # -- articulation + self._root_physx_view = self._physics_sim_view.create_articulation_view(root_prim_path_expr.replace(".*", "*")) + + # check if the articulation was created + if self._root_physx_view._backend is None: + raise RuntimeError(f"Failed to create articulation at: {root_prim_path_expr}. Please check PhysX logs.") + + # log information about the articulation + omni.log.info(f"Articulation initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.") + omni.log.info(f"Is fixed root: {self.is_fixed_base}") + omni.log.info(f"Number of bodies: {self.num_bodies}") + omni.log.info(f"Body names: {self.body_names}") + omni.log.info(f"Number of joints: {self.num_joints}") + omni.log.info(f"Joint names: {self.joint_names}") + omni.log.info(f"Number of fixed tendons: {self.num_fixed_tendons}") + + # container for data access + self._data = ArticulationData(self.root_physx_view, self.device) + + # create buffers + self._create_buffers() + # process configuration + self._process_cfg() + self._process_actuators_cfg() + self._process_fixed_tendons() + # validate configuration + self._validate_cfg() + # update the robot data + self.update(0.0) + # log joint information + self._log_articulation_info() + + def _create_buffers(self): + # constants + self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + + # external forces and torques + self.has_external_wrench = False + self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device) + self._external_torque_b = torch.zeros_like(self._external_force_b) + + # asset named data + self._data.joint_names = self.joint_names + self._data.body_names = self.body_names + # tendon names are set in _process_fixed_tendons function + + # -- joint properties + self._data.default_joint_pos_limits = self.root_physx_view.get_dof_limits().to(self.device).clone() + self._data.default_joint_stiffness = self.root_physx_view.get_dof_stiffnesses().to(self.device).clone() + self._data.default_joint_damping = self.root_physx_view.get_dof_dampings().to(self.device).clone() + self._data.default_joint_armature = self.root_physx_view.get_dof_armatures().to(self.device).clone() + self._data.default_joint_friction_coeff = ( + self.root_physx_view.get_dof_friction_coefficients().to(self.device).clone() + ) + + self._data.joint_pos_limits = self._data.default_joint_pos_limits.clone() + self._data.joint_vel_limits = self.root_physx_view.get_dof_max_velocities().to(self.device).clone() + self._data.joint_effort_limits = self.root_physx_view.get_dof_max_forces().to(self.device).clone() + self._data.joint_stiffness = self._data.default_joint_stiffness.clone() + self._data.joint_damping = self._data.default_joint_damping.clone() + self._data.joint_armature = self._data.default_joint_armature.clone() + self._data.joint_friction_coeff = self._data.default_joint_friction_coeff.clone() + + # -- body properties + self._data.default_mass = self.root_physx_view.get_masses().clone() + self._data.default_inertia = self.root_physx_view.get_inertias().clone() + + # -- joint commands (sent to the actuator from the user) + self._data.joint_pos_target = torch.zeros(self.num_instances, self.num_joints, device=self.device) + self._data.joint_vel_target = torch.zeros_like(self._data.joint_pos_target) + self._data.joint_effort_target = torch.zeros_like(self._data.joint_pos_target) + # -- joint commands (sent to the simulation after actuator processing) + self._joint_pos_target_sim = torch.zeros_like(self._data.joint_pos_target) + self._joint_vel_target_sim = torch.zeros_like(self._data.joint_pos_target) + self._joint_effort_target_sim = torch.zeros_like(self._data.joint_pos_target) + + # -- computed joint efforts from the actuator models + self._data.computed_torque = torch.zeros_like(self._data.joint_pos_target) + self._data.applied_torque = torch.zeros_like(self._data.joint_pos_target) + + # -- other data that are filled based on explicit actuator models + self._data.soft_joint_vel_limits = torch.zeros(self.num_instances, self.num_joints, device=self.device) + self._data.gear_ratio = torch.ones(self.num_instances, self.num_joints, device=self.device) + + # soft joint position limits (recommended not to be too close to limits). + joint_pos_mean = (self._data.joint_pos_limits[..., 0] + self._data.joint_pos_limits[..., 1]) / 2 + joint_pos_range = self._data.joint_pos_limits[..., 1] - self._data.joint_pos_limits[..., 0] + soft_limit_factor = self.cfg.soft_joint_pos_limit_factor + # add to data + self._data.soft_joint_pos_limits = torch.zeros(self.num_instances, self.num_joints, 2, device=self.device) + self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor + self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor + + def _process_cfg(self): + """Post processing of configuration parameters.""" + # default state + # -- root state + # note: we cast to tuple to avoid torch/numpy type mismatch. + default_root_state = ( + tuple(self.cfg.init_state.pos) + + tuple(self.cfg.init_state.rot) + + tuple(self.cfg.init_state.lin_vel) + + tuple(self.cfg.init_state.ang_vel) + ) + default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device) + self._data.default_root_state = default_root_state.repeat(self.num_instances, 1) + + # -- joint state + self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device) + self._data.default_joint_vel = torch.zeros_like(self._data.default_joint_pos) + # joint pos + indices_list, _, values_list = string_utils.resolve_matching_names_values( + self.cfg.init_state.joint_pos, self.joint_names + ) + self._data.default_joint_pos[:, indices_list] = torch.tensor(values_list, device=self.device) + # joint vel + indices_list, _, values_list = string_utils.resolve_matching_names_values( + self.cfg.init_state.joint_vel, self.joint_names + ) + self._data.default_joint_vel[:, indices_list] = torch.tensor(values_list, device=self.device) + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + self._root_physx_view = None + + """ + Internal helpers -- Actuators. + """ + + def _process_actuators_cfg(self): + """Process and apply articulation joint properties.""" + # create actuators + self.actuators = dict() + # flag for implicit actuators + # if this is false, we by-pass certain checks when doing actuator-related operations + self._has_implicit_actuators = False + + # iterate over all actuator configurations + for actuator_name, actuator_cfg in self.cfg.actuators.items(): + # type annotation for type checkers + actuator_cfg: ActuatorBaseCfg + # create actuator group + joint_ids, joint_names = self.find_joints(actuator_cfg.joint_names_expr) + # check if any joints are found + if len(joint_names) == 0: + raise ValueError( + f"No joints found for actuator group: {actuator_name} with joint name expression:" + f" {actuator_cfg.joint_names_expr}." + ) + # resolve joint indices + # we pass a slice if all joints are selected to avoid indexing overhead + if len(joint_names) == self.num_joints: + joint_ids = slice(None) + else: + joint_ids = torch.tensor(joint_ids, device=self.device) + # create actuator collection + # note: for efficiency avoid indexing when over all indices + actuator: ActuatorBase = actuator_cfg.class_type( + cfg=actuator_cfg, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=self.num_instances, + device=self.device, + stiffness=self._data.default_joint_stiffness[:, joint_ids], + damping=self._data.default_joint_damping[:, joint_ids], + armature=self._data.default_joint_armature[:, joint_ids], + friction=self._data.default_joint_friction_coeff[:, joint_ids], + effort_limit=self._data.joint_effort_limits[:, joint_ids], + velocity_limit=self._data.joint_vel_limits[:, joint_ids], + ) + # log information on actuator groups + model_type = "implicit" if actuator.is_implicit_model else "explicit" + omni.log.info( + f"Actuator collection: {actuator_name} with model '{actuator_cfg.class_type.__name__}'" + f" (type: {model_type}) and joint names: {joint_names} [{joint_ids}]." + ) + # store actuator group + self.actuators[actuator_name] = actuator + # set the passed gains and limits into the simulation + if isinstance(actuator, ImplicitActuator): + self._has_implicit_actuators = True + # the gains and limits are set into the simulation since actuator model is implicit + self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices) + self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices) + else: + # the gains and limits are processed by the actuator model + # we set gains to zero, and torque limit to a high value in simulation to avoid any interference + self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices) + self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices) + + # Set common properties into the simulation + self.write_joint_effort_limit_to_sim(actuator.effort_limit_sim, joint_ids=actuator.joint_indices) + self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim, joint_ids=actuator.joint_indices) + self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices) + self.write_joint_friction_coefficient_to_sim(actuator.friction, joint_ids=actuator.joint_indices) + + # Store the configured values from the actuator model + # note: this is the value configured in the actuator model (for implicit and explicit actuators) + self._data.default_joint_stiffness[:, actuator.joint_indices] = actuator.stiffness + self._data.default_joint_damping[:, actuator.joint_indices] = actuator.damping + self._data.default_joint_armature[:, actuator.joint_indices] = actuator.armature + self._data.default_joint_friction_coeff[:, actuator.joint_indices] = actuator.friction + + # perform some sanity checks to ensure actuators are prepared correctly + total_act_joints = sum(actuator.num_joints for actuator in self.actuators.values()) + if total_act_joints != (self.num_joints - self.num_fixed_tendons): + omni.log.warn( + "Not all actuators are configured! Total number of actuated joints not equal to number of" + f" joints available: {total_act_joints} != {self.num_joints - self.num_fixed_tendons}." + ) + + def _process_fixed_tendons(self): + """Process fixed tendons.""" + # create a list to store the fixed tendon names + self._fixed_tendon_names = list() + + # parse fixed tendons properties if they exist + if self.num_fixed_tendons > 0: + stage = stage_utils.get_current_stage() + joint_paths = self.root_physx_view.dof_paths[0] + + # iterate over all joints to find tendons attached to them + for j in range(self.num_joints): + usd_joint_path = joint_paths[j] + # check whether joint has tendons - tendon name follows the joint name it is attached to + joint = UsdPhysics.Joint.Get(stage, usd_joint_path) + if joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAxisRootAPI): + joint_name = usd_joint_path.split("/")[-1] + self._fixed_tendon_names.append(joint_name) + + # store the fixed tendon names + self._data.fixed_tendon_names = self._fixed_tendon_names + + # store the current USD fixed tendon properties + self._data.default_fixed_tendon_stiffness = self.root_physx_view.get_fixed_tendon_stiffnesses().clone() + self._data.default_fixed_tendon_damping = self.root_physx_view.get_fixed_tendon_dampings().clone() + self._data.default_fixed_tendon_limit_stiffness = ( + self.root_physx_view.get_fixed_tendon_limit_stiffnesses().clone() + ) + self._data.default_fixed_tendon_pos_limits = self.root_physx_view.get_fixed_tendon_limits().clone() + self._data.default_fixed_tendon_rest_length = self.root_physx_view.get_fixed_tendon_rest_lengths().clone() + self._data.default_fixed_tendon_offset = self.root_physx_view.get_fixed_tendon_offsets().clone() + + # store a copy of the default values for the fixed tendons + self._data.fixed_tendon_stiffness = self._data.default_fixed_tendon_stiffness.clone() + self._data.fixed_tendon_damping = self._data.default_fixed_tendon_damping.clone() + self._data.fixed_tendon_limit_stiffness = self._data.default_fixed_tendon_limit_stiffness.clone() + self._data.fixed_tendon_pos_limits = self._data.default_fixed_tendon_pos_limits.clone() + self._data.fixed_tendon_rest_length = self._data.default_fixed_tendon_rest_length.clone() + self._data.fixed_tendon_offset = self._data.default_fixed_tendon_offset.clone() + + def _apply_actuator_model(self): + """Processes joint commands for the articulation by forwarding them to the actuators. + + The actions are first processed using actuator models. Depending on the robot configuration, + the actuator models compute the joint level simulation commands and sets them into the PhysX buffers. + """ + # process actions per group + for actuator in self.actuators.values(): + # prepare input for actuator model based on cached data + # TODO : A tensor dict would be nice to do the indexing of all tensors together + control_action = ArticulationActions( + joint_positions=self._data.joint_pos_target[:, actuator.joint_indices], + joint_velocities=self._data.joint_vel_target[:, actuator.joint_indices], + joint_efforts=self._data.joint_effort_target[:, actuator.joint_indices], + joint_indices=actuator.joint_indices, + ) + # compute joint command from the actuator model + control_action = actuator.compute( + control_action, + joint_pos=self._data.joint_pos[:, actuator.joint_indices], + joint_vel=self._data.joint_vel[:, actuator.joint_indices], + ) + # update targets (these are set into the simulation) + if control_action.joint_positions is not None: + self._joint_pos_target_sim[:, actuator.joint_indices] = control_action.joint_positions + if control_action.joint_velocities is not None: + self._joint_vel_target_sim[:, actuator.joint_indices] = control_action.joint_velocities + if control_action.joint_efforts is not None: + self._joint_effort_target_sim[:, actuator.joint_indices] = control_action.joint_efforts + # update state of the actuator model + # -- torques + self._data.computed_torque[:, actuator.joint_indices] = actuator.computed_effort + self._data.applied_torque[:, actuator.joint_indices] = actuator.applied_effort + # -- actuator data + self._data.soft_joint_vel_limits[:, actuator.joint_indices] = actuator.velocity_limit + # TODO: find a cleaner way to handle gear ratio. Only needed for variable gear ratio actuators. + if hasattr(actuator, "gear_ratio"): + self._data.gear_ratio[:, actuator.joint_indices] = actuator.gear_ratio + + """ + Internal helpers -- Debugging. + """ + + def _validate_cfg(self): + """Validate the configuration after processing. + + Note: + This function should be called only after the configuration has been processed and the buffers have been + created. Otherwise, some settings that are altered during processing may not be validated. + For instance, the actuator models may change the joint max velocity limits. + """ + # check that the default values are within the limits + joint_pos_limits = self.root_physx_view.get_dof_limits()[0].to(self.device) + out_of_range = self._data.default_joint_pos[0] < joint_pos_limits[:, 0] + out_of_range |= self._data.default_joint_pos[0] > joint_pos_limits[:, 1] + violated_indices = torch.nonzero(out_of_range, as_tuple=False).squeeze(-1) + # throw error if any of the default joint positions are out of the limits + if len(violated_indices) > 0: + # prepare message for violated joints + msg = "The following joints have default positions out of the limits: \n" + for idx in violated_indices: + joint_name = self.data.joint_names[idx] + joint_limit = joint_pos_limits[idx] + joint_pos = self.data.default_joint_pos[0, idx] + # add to message + msg += f"\t- '{joint_name}': {joint_pos:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n" + raise ValueError(msg) + + # check that the default joint velocities are within the limits + joint_max_vel = self.root_physx_view.get_dof_max_velocities()[0].to(self.device) + out_of_range = torch.abs(self._data.default_joint_vel[0]) > joint_max_vel + violated_indices = torch.nonzero(out_of_range, as_tuple=False).squeeze(-1) + if len(violated_indices) > 0: + # prepare message for violated joints + msg = "The following joints have default velocities out of the limits: \n" + for idx in violated_indices: + joint_name = self.data.joint_names[idx] + joint_limit = [-joint_max_vel[idx], joint_max_vel[idx]] + joint_vel = self.data.default_joint_vel[0, idx] + # add to message + msg += f"\t- '{joint_name}': {joint_vel:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n" + raise ValueError(msg) + + def _log_articulation_info(self): + """Log information about the articulation. + + Note: We purposefully read the values from the simulator to ensure that the values are configured as expected. + """ + # read out all joint parameters from simulation + # -- gains + stiffnesses = self.root_physx_view.get_dof_stiffnesses()[0].tolist() + dampings = self.root_physx_view.get_dof_dampings()[0].tolist() + # -- properties + armatures = self.root_physx_view.get_dof_armatures()[0].tolist() + frictions = self.root_physx_view.get_dof_friction_coefficients()[0].tolist() + # -- limits + position_limits = self.root_physx_view.get_dof_limits()[0].tolist() + velocity_limits = self.root_physx_view.get_dof_max_velocities()[0].tolist() + effort_limits = self.root_physx_view.get_dof_max_forces()[0].tolist() + # create table for term information + joint_table = PrettyTable() + joint_table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})" + joint_table.field_names = [ + "Index", + "Name", + "Stiffness", + "Damping", + "Armature", + "Friction", + "Position Limits", + "Velocity Limits", + "Effort Limits", + ] + joint_table.float_format = ".3" + joint_table.custom_format["Position Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]" + # set alignment of table columns + joint_table.align["Name"] = "l" + # add info on each term + for index, name in enumerate(self.joint_names): + joint_table.add_row([ + index, + name, + stiffnesses[index], + dampings[index], + armatures[index], + frictions[index], + position_limits[index], + velocity_limits[index], + effort_limits[index], + ]) + # convert table to string + omni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string()) + + # read out all tendon parameters from simulation + if self.num_fixed_tendons > 0: + # -- gains + ft_stiffnesses = self.root_physx_view.get_fixed_tendon_stiffnesses()[0].tolist() + ft_dampings = self.root_physx_view.get_fixed_tendon_dampings()[0].tolist() + # -- limits + ft_limit_stiffnesses = self.root_physx_view.get_fixed_tendon_limit_stiffnesses()[0].tolist() + ft_limits = self.root_physx_view.get_fixed_tendon_limits()[0].tolist() + ft_rest_lengths = self.root_physx_view.get_fixed_tendon_rest_lengths()[0].tolist() + ft_offsets = self.root_physx_view.get_fixed_tendon_offsets()[0].tolist() + # create table for term information + tendon_table = PrettyTable() + tendon_table.title = f"Simulation Fixed Tendon Information (Prim path: {self.cfg.prim_path})" + tendon_table.field_names = [ + "Index", + "Stiffness", + "Damping", + "Limit Stiffness", + "Limits", + "Rest Length", + "Offset", + ] + tendon_table.float_format = ".3" + joint_table.custom_format["Limits"] = lambda f, v: f"[{v[0]:.3f}, {v[1]:.3f}]" + # add info on each term + for index in range(self.num_fixed_tendons): + tendon_table.add_row([ + index, + ft_stiffnesses[index], + ft_dampings[index], + ft_limit_stiffnesses[index], + ft_limits[index], + ft_rest_lengths[index], + ft_offsets[index], + ]) + # convert table to string + omni.log.info(f"Simulation parameters for tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string()) + + """ + Deprecated methods. + """ + + def write_joint_friction_to_sim( + self, + joint_friction: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Write joint friction coefficients into the simulation. + + .. deprecated:: 2.1.0 + Please use :meth:`write_joint_friction_coefficient_to_sim` instead. + """ + omni.log.warn( + "The function 'write_joint_friction_to_sim' will be deprecated in a future release. Please" + " use 'write_joint_friction_coefficient_to_sim' instead." + ) + self.write_joint_friction_coefficient_to_sim(joint_friction, joint_ids=joint_ids, env_ids=env_ids) + + def write_joint_limits_to_sim( + self, + limits: torch.Tensor | float, + joint_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + warn_limit_violation: bool = True, + ): + """Write joint limits into the simulation. + + .. deprecated:: 2.1.0 + Please use :meth:`write_joint_position_limit_to_sim` instead. + """ + omni.log.warn( + "The function 'write_joint_limits_to_sim' will be deprecated in a future release. Please" + " use 'write_joint_position_limit_to_sim' instead." + ) + self.write_joint_position_limit_to_sim( + limits, joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=warn_limit_violation + ) + + def set_fixed_tendon_limit( + self, + limit: torch.Tensor, + fixed_tendon_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set fixed tendon position limits into internal buffers. + + .. deprecated:: 2.1.0 + Please use :meth:`set_fixed_tendon_position_limit` instead. + """ + omni.log.warn( + "The function 'set_fixed_tendon_limit' will be deprecated in a future release. Please" + " use 'set_fixed_tendon_position_limit' instead." + ) + self.set_fixed_tendon_position_limit(limit, fixed_tendon_ids=fixed_tendon_ids, env_ids=env_ids) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_cfg.py new file mode 100644 index 00000000000..0fcf733d85b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_cfg.py @@ -0,0 +1,66 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.actuators import ActuatorBaseCfg +from isaaclab.utils import configclass + +from ..asset_base_cfg import AssetBaseCfg +from .articulation import Articulation + + +@configclass +class ArticulationCfg(AssetBaseCfg): + """Configuration parameters for an articulation.""" + + @configclass + class InitialStateCfg(AssetBaseCfg.InitialStateCfg): + """Initial state of the articulation.""" + + # root velocity + lin_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + # joint state + joint_pos: dict[str, float] = {".*": 0.0} + """Joint positions of the joints. Defaults to 0.0 for all joints.""" + joint_vel: dict[str, float] = {".*": 0.0} + """Joint velocities of the joints. Defaults to 0.0 for all joints.""" + + ## + # Initialize configurations. + ## + + class_type: type = Articulation + + articulation_root_prim_path: str | None = None + """Path to the articulation root prim in the USD file. + + If not provided will search for a prim with the ArticulationRootAPI. Should start with a slash. + """ + + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state.""" + + soft_joint_pos_limit_factor: float = 1.0 + """Fraction specifying the range of joint position limits (parsed from the asset) to use. Defaults to 1.0. + + The soft joint position limits are scaled by this factor to specify a safety region within the simulated + joint position limits. This isn't used by the simulation, but is useful for learning agents to prevent the joint + positions from violating the limits, such as for termination conditions. + + The soft joint position limits are accessible through the :attr:`ArticulationData.soft_joint_pos_limits` attribute. + """ + + actuators: dict[str, ActuatorBaseCfg] = MISSING + """Actuators for the robot with corresponding joint names.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_data.py new file mode 100644 index 00000000000..77499add9cb --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/articulation/articulation_data.py @@ -0,0 +1,1077 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +import weakref + +import omni.log +import omni.physics.tensors.impl.api as physx +from isaacsim.core.simulation_manager import SimulationManager + +import isaaclab.utils.math as math_utils +from isaaclab.utils.buffers import TimestampedBuffer + + +class ArticulationData: + """Data container for an articulation. + + This class contains the data for an articulation in the simulation. The data includes the state of + the root rigid body, the state of all the bodies in the articulation, and the joint state. The data is + stored in the simulation world frame unless otherwise specified. + + An articulation is comprised of multiple rigid bodies or links. For a rigid body, there are two frames + of reference that are used: + + - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim + with the rigid body schema. + - Center of mass frame: The frame of reference of the center of mass of the rigid body. + + Depending on the settings, the two frames may not coincide with each other. In the robotics sense, the actor frame + can be interpreted as the link frame. + """ + + def __init__(self, root_physx_view: physx.ArticulationView, device: str): + """Initializes the articulation data. + + Args: + root_physx_view: The root articulation view. + device: The device used for processing. + """ + # Set the parameters + self.device = device + # Set the root articulation view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.ArticulationView = weakref.proxy(root_physx_view) + + # Set initial time stamp + self._sim_timestamp = 0.0 + + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + gravity = self._physics_sim_view.get_gravity() + # Convert to direction vector + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + + # Initialize constants + self.GRAVITY_VEC_W = gravity_dir.repeat(self._root_physx_view.count, 1) + self.FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self._root_physx_view.count, 1) + + # Initialize history for finite differencing + self._previous_joint_vel = self._root_physx_view.get_dof_velocities().clone() + + # Initialize the lazy buffers. + # -- link frame w.r.t. world frame + self._root_link_pose_w = TimestampedBuffer() + self._root_link_vel_w = TimestampedBuffer() + self._body_link_pose_w = TimestampedBuffer() + self._body_link_vel_w = TimestampedBuffer() + # -- com frame w.r.t. link frame + self._body_com_pose_b = TimestampedBuffer() + # -- com frame w.r.t. world frame + self._root_com_pose_w = TimestampedBuffer() + self._root_com_vel_w = TimestampedBuffer() + self._body_com_pose_w = TimestampedBuffer() + self._body_com_vel_w = TimestampedBuffer() + self._body_com_acc_w = TimestampedBuffer() + # -- combined state (these are cached as they concatenate) + self._root_state_w = TimestampedBuffer() + self._root_link_state_w = TimestampedBuffer() + self._root_com_state_w = TimestampedBuffer() + self._body_state_w = TimestampedBuffer() + self._body_link_state_w = TimestampedBuffer() + self._body_com_state_w = TimestampedBuffer() + # -- joint state + self._joint_pos = TimestampedBuffer() + self._joint_vel = TimestampedBuffer() + self._joint_acc = TimestampedBuffer() + self._body_incoming_joint_wrench_b = TimestampedBuffer() + + def update(self, dt: float): + # update the simulation timestamp + self._sim_timestamp += dt + # Trigger an update of the joint acceleration buffer at a higher frequency + # since we do finite differencing. + self.joint_acc + + ## + # Names. + ## + + body_names: list[str] = None + """Body names in the order parsed by the simulation view.""" + + joint_names: list[str] = None + """Joint names in the order parsed by the simulation view.""" + + fixed_tendon_names: list[str] = None + """Fixed tendon names in the order parsed by the simulation view.""" + + ## + # Defaults - Initial state. + ## + + default_root_state: torch.Tensor = None + """Default root state ``[pos, quat, lin_vel, ang_vel]`` in the local environment frame. Shape is (num_instances, 13). + + The position and quaternion are of the articulation root's actor frame. Meanwhile, the linear and angular + velocities are of its center of mass frame. + + This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter. + """ + + default_joint_pos: torch.Tensor = None + """Default joint positions of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter. + """ + + default_joint_vel: torch.Tensor = None + """Default joint velocities of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter. + """ + + ## + # Defaults - Physical properties. + ## + + default_mass: torch.Tensor = None + """Default mass for all the bodies in the articulation. Shape is (num_instances, num_bodies). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_inertia: torch.Tensor = None + """Default inertia for all the bodies in the articulation. Shape is (num_instances, num_bodies, 9). + + The inertia is the inertia tensor relative to the center of mass frame. The values are stored in + the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_joint_stiffness: torch.Tensor = None + """Default joint stiffness of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.stiffness` + parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization, + is used. + + .. attention:: + The default stiffness is the value configured by the user or the value parsed from the USD schema. + It should not be confused with :attr:`joint_stiffness`, which is the value set into the simulation. + """ + + default_joint_damping: torch.Tensor = None + """Default joint damping of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.damping` + parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization, + is used. + + .. attention:: + The default stiffness is the value configured by the user or the value parsed from the USD schema. + It should not be confused with :attr:`joint_damping`, which is the value set into the simulation. + """ + + default_joint_armature: torch.Tensor = None + """Default joint armature of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.armature` + parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization, + is used. + """ + + default_joint_friction_coeff: torch.Tensor = None + """Default joint friction coefficient of all joints. Shape is (num_instances, num_joints). + + This quantity is configured through the actuator model's :attr:`isaaclab.actuators.ActuatorBaseCfg.friction` + parameter. If the parameter's value is None, the value parsed from the USD schema, at the time of initialization, + is used. + """ + + default_joint_pos_limits: torch.Tensor = None + """Default joint position limits of all joints. Shape is (num_instances, num_joints, 2). + + The limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_stiffness: torch.Tensor = None + """Default tendon stiffness of all tendons. Shape is (num_instances, num_fixed_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_damping: torch.Tensor = None + """Default tendon damping of all tendons. Shape is (num_instances, num_fixed_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_limit_stiffness: torch.Tensor = None + """Default tendon limit stiffness of all tendons. Shape is (num_instances, num_fixed_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_rest_length: torch.Tensor = None + """Default tendon rest length of all tendons. Shape is (num_instances, num_fixed_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_offset: torch.Tensor = None + """Default tendon offset of all tendons. Shape is (num_instances, num_fixed_tendons). + + This quantity is parsed from the USD schema at the time of initialization. + """ + + default_fixed_tendon_pos_limits: torch.Tensor = None + """Default tendon position limits of all tendons. Shape is (num_instances, num_fixed_tendons, 2). + + The position limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of + initialization. + """ + + ## + # Joint commands -- Set into simulation. + ## + + joint_pos_target: torch.Tensor = None + """Joint position targets commanded by the user. Shape is (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`), + which are then set into the simulation. + """ + + joint_vel_target: torch.Tensor = None + """Joint velocity targets commanded by the user. Shape is (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`), + which are then set into the simulation. + """ + + joint_effort_target: torch.Tensor = None + """Joint effort targets commanded by the user. Shape is (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`), + which are then set into the simulation. + """ + + ## + # Joint commands -- Explicit actuators. + ## + + computed_torque: torch.Tensor = None + """Joint torques computed from the actuator model (before clipping). Shape is (num_instances, num_joints). + + This quantity is the raw torque output from the actuator mode, before any clipping is applied. + It is exposed for users who want to inspect the computations inside the actuator model. + For instance, to penalize the learning agent for a difference between the computed and applied torques. + """ + + applied_torque: torch.Tensor = None + """Joint torques applied from the actuator model (after clipping). Shape is (num_instances, num_joints). + + These torques are set into the simulation, after clipping the :attr:`computed_torque` based on the + actuator model. + """ + + ## + # Joint properties. + ## + + joint_stiffness: torch.Tensor = None + """Joint stiffness provided to the simulation. Shape is (num_instances, num_joints). + + In the case of explicit actuators, the value for the corresponding joints is zero. + """ + + joint_damping: torch.Tensor = None + """Joint damping provided to the simulation. Shape is (num_instances, num_joints) + + In the case of explicit actuators, the value for the corresponding joints is zero. + """ + + joint_armature: torch.Tensor = None + """Joint armature provided to the simulation. Shape is (num_instances, num_joints).""" + + joint_friction_coeff: torch.Tensor = None + """Joint friction coefficient provided to the simulation. Shape is (num_instances, num_joints).""" + + joint_pos_limits: torch.Tensor = None + """Joint position limits provided to the simulation. Shape is (num_instances, num_joints, 2). + + The limits are in the order :math:`[lower, upper]`. + """ + + joint_vel_limits: torch.Tensor = None + """Joint maximum velocity provided to the simulation. Shape is (num_instances, num_joints).""" + + joint_effort_limits: torch.Tensor = None + """Joint maximum effort provided to the simulation. Shape is (num_instances, num_joints).""" + + ## + # Joint properties - Custom. + ## + + soft_joint_pos_limits: torch.Tensor = None + r"""Soft joint positions limits for all joints. Shape is (num_instances, num_joints, 2). + + The limits are in the order :math:`[lower, upper]`.The soft joint position limits are computed as + a sub-region of the :attr:`joint_pos_limits` based on the + :attr:`~isaaclab.assets.ArticulationCfg.soft_joint_pos_limit_factor` parameter. + + Consider the joint position limits :math:`[lower, upper]` and the soft joint position limits + :math:`[soft_lower, soft_upper]`. The soft joint position limits are computed as: + + .. math:: + + soft\_lower = (lower + upper) / 2 - factor * (upper - lower) / 2 + soft\_upper = (lower + upper) / 2 + factor * (upper - lower) / 2 + + The soft joint position limits help specify a safety region around the joint limits. It isn't used by the + simulation, but is useful for learning agents to prevent the joint positions from violating the limits. + """ + + soft_joint_vel_limits: torch.Tensor = None + """Soft joint velocity limits for all joints. Shape is (num_instances, num_joints). + + These are obtained from the actuator model. It may differ from :attr:`joint_vel_limits` if the actuator model + has a variable velocity limit model. For instance, in a variable gear ratio actuator model. + """ + + gear_ratio: torch.Tensor = None + """Gear ratio for relating motor torques to applied Joint torques. Shape is (num_instances, num_joints).""" + + ## + # Fixed tendon properties. + ## + + fixed_tendon_stiffness: torch.Tensor = None + """Fixed tendon stiffness provided to the simulation. Shape is (num_instances, num_fixed_tendons).""" + + fixed_tendon_damping: torch.Tensor = None + """Fixed tendon damping provided to the simulation. Shape is (num_instances, num_fixed_tendons).""" + + fixed_tendon_limit_stiffness: torch.Tensor = None + """Fixed tendon limit stiffness provided to the simulation. Shape is (num_instances, num_fixed_tendons).""" + + fixed_tendon_rest_length: torch.Tensor = None + """Fixed tendon rest length provided to the simulation. Shape is (num_instances, num_fixed_tendons).""" + + fixed_tendon_offset: torch.Tensor = None + """Fixed tendon offset provided to the simulation. Shape is (num_instances, num_fixed_tendons).""" + + fixed_tendon_pos_limits: torch.Tensor = None + """Fixed tendon position limits provided to the simulation. Shape is (num_instances, num_fixed_tendons, 2).""" + + ## + # Root state properties. + ## + + @property + def root_link_pose_w(self) -> torch.Tensor: + """Root link pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 7). + + This quantity is the pose of the articulation root's actor frame relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._root_link_pose_w.timestamp < self._sim_timestamp: + # read data from simulation + pose = self._root_physx_view.get_root_transforms().clone() + pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") + # set the buffer data and timestamp + self._root_link_pose_w.data = pose + self._root_link_pose_w.timestamp = self._sim_timestamp + + return self._root_link_pose_w.data + + @property + def root_link_vel_w(self) -> torch.Tensor: + """Root link velocity ``[lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 6). + + This quantity contains the linear and angular velocities of the articulation root's actor frame + relative to the world. + """ + if self._root_link_vel_w.timestamp < self._sim_timestamp: + # read the CoM velocity + vel = self.root_com_vel_w.clone() + # adjust linear velocity to link from center of mass + vel[:, :3] += torch.linalg.cross( + vel[:, 3:], math_utils.quat_rotate(self.root_link_quat_w, -self.body_com_pos_b[:, 0]), dim=-1 + ) + # set the buffer data and timestamp + self._root_link_vel_w.data = vel + self._root_link_vel_w.timestamp = self._sim_timestamp + + return self._root_link_vel_w.data + + @property + def root_com_pose_w(self) -> torch.Tensor: + """Root center of mass pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 7). + + This quantity is the pose of the articulation root's center of mass frame relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._root_com_pose_w.timestamp < self._sim_timestamp: + # apply local transform to center of mass frame + pos, quat = math_utils.combine_frame_transforms( + self.root_link_pos_w, self.root_link_quat_w, self.body_com_pos_b[:, 0], self.body_com_quat_b[:, 0] + ) + # set the buffer data and timestamp + self._root_com_pose_w.data = torch.cat((pos, quat), dim=-1) + self._root_com_pose_w.timestamp = self._sim_timestamp + + return self._root_com_pose_w.data + + @property + def root_com_vel_w(self) -> torch.Tensor: + """Root center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 6). + + This quantity contains the linear and angular velocities of the articulation root's center of mass frame + relative to the world. + """ + if self._root_com_vel_w.timestamp < self._sim_timestamp: + self._root_com_vel_w.data = self._root_physx_view.get_root_velocities() + self._root_com_vel_w.timestamp = self._sim_timestamp + + return self._root_com_vel_w.data + + @property + def root_state_w(self): + """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + + The position and quaternion are of the articulation root's actor frame relative to the world. Meanwhile, + the linear and angular velocities are of the articulation root's center of mass frame. + """ + if self._root_state_w.timestamp < self._sim_timestamp: + self._root_state_w.data = torch.cat((self.root_link_pose_w, self.root_com_vel_w), dim=-1) + self._root_state_w.timestamp = self._sim_timestamp + + return self._root_state_w.data + + @property + def root_link_state_w(self): + """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + + The position, quaternion, and linear/angular velocity are of the articulation root's actor frame relative to the + world. + """ + if self._root_link_state_w.timestamp < self._sim_timestamp: + self._root_link_state_w.data = torch.cat((self.root_link_pose_w, self.root_link_vel_w), dim=-1) + self._root_link_state_w.timestamp = self._sim_timestamp + + return self._root_link_state_w.data + + @property + def root_com_state_w(self): + """Root center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + + The position, quaternion, and linear/angular velocity are of the articulation root link's center of mass frame + relative to the world. Center of mass frame is assumed to be the same orientation as the link rather than the + orientation of the principle inertia. + """ + if self._root_com_state_w.timestamp < self._sim_timestamp: + self._root_com_state_w.data = torch.cat((self.root_com_pose_w, self.root_com_vel_w), dim=-1) + self._root_com_state_w.timestamp = self._sim_timestamp + + return self._root_com_state_w.data + + ## + # Body state properties. + ## + + @property + def body_link_pose_w(self) -> torch.Tensor: + """Body link pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances, num_bodies, 7). + + This quantity is the pose of the articulation links' actor frame relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._body_link_pose_w.timestamp < self._sim_timestamp: + # perform forward kinematics (shouldn't cause overhead if it happened already) + self._physics_sim_view.update_articulations_kinematic() + # read data from simulation + poses = self._root_physx_view.get_link_transforms().clone() + poses[..., 3:7] = math_utils.convert_quat(poses[..., 3:7], to="wxyz") + # set the buffer data and timestamp + self._body_link_pose_w.data = poses + self._body_link_pose_w.timestamp = self._sim_timestamp + + return self._body_link_pose_w.data + + @property + def body_link_vel_w(self) -> torch.Tensor: + """Body link velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_bodies, 6). + + This quantity contains the linear and angular velocities of the articulation links' actor frame + relative to the world. + """ + if self._body_link_vel_w.timestamp < self._sim_timestamp: + # read data from simulation + velocities = self.body_com_vel_w.clone() + # adjust linear velocity to link from center of mass + velocities[..., :3] += torch.linalg.cross( + velocities[..., 3:], math_utils.quat_rotate(self.body_link_quat_w, -self.body_com_pos_b), dim=-1 + ) + # set the buffer data and timestamp + self._body_link_vel_w.data = velocities + self._body_link_vel_w.timestamp = self._sim_timestamp + + return self._body_link_vel_w.data + + @property + def body_com_pose_w(self) -> torch.Tensor: + """Body center of mass pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances, num_bodies, 7). + + This quantity is the pose of the center of mass frame of the articulation links relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._body_com_pose_w.timestamp < self._sim_timestamp: + # apply local transform to center of mass frame + pos, quat = math_utils.combine_frame_transforms( + self.body_link_pos_w, self.body_link_quat_w, self.body_com_pos_b, self.body_com_quat_b + ) + # set the buffer data and timestamp + self._body_com_pose_w.data = torch.cat((pos, quat), dim=-1) + self._body_com_pose_w.timestamp = self._sim_timestamp + + return self._body_com_pose_w.data + + @property + def body_com_vel_w(self) -> torch.Tensor: + """Body center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_bodies, 6). + + This quantity contains the linear and angular velocities of the articulation links' center of mass frame + relative to the world. + """ + if self._body_com_vel_w.timestamp < self._sim_timestamp: + self._body_com_vel_w.data = self._root_physx_view.get_link_velocities() + self._body_com_vel_w.timestamp = self._sim_timestamp + + return self._body_com_vel_w.data + + @property + def body_state_w(self): + """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. + Shape is (num_instances, num_bodies, 13). + + The position and quaternion are of all the articulation links' actor frame. Meanwhile, the linear and angular + velocities are of the articulation links's center of mass frame. + """ + if self._body_state_w.timestamp < self._sim_timestamp: + self._body_state_w.data = torch.cat((self.body_link_pose_w, self.body_com_vel_w), dim=-1) + self._body_state_w.timestamp = self._sim_timestamp + + return self._body_state_w.data + + @property + def body_link_state_w(self): + """State of all bodies' link frame`[pos, quat, lin_vel, ang_vel]` in simulation world frame. + Shape is (num_instances, num_bodies, 13). + + The position, quaternion, and linear/angular velocity are of the body's link frame relative to the world. + """ + if self._body_link_state_w.timestamp < self._sim_timestamp: + self._body_link_state_w.data = torch.cat((self.body_link_pose_w, self.body_link_vel_w), dim=-1) + self._body_link_state_w.timestamp = self._sim_timestamp + + return self._body_link_state_w.data + + @property + def body_com_state_w(self): + """State of all bodies center of mass `[pos, quat, lin_vel, ang_vel]` in simulation world frame. + Shape is (num_instances, num_bodies, 13). + + The position, quaternion, and linear/angular velocity are of the body's center of mass frame relative to the + world. Center of mass frame is assumed to be the same orientation as the link rather than the orientation of the + principle inertia. + """ + if self._body_com_state_w.timestamp < self._sim_timestamp: + self._body_com_state_w.data = torch.cat((self.body_com_pose_w, self.body_com_vel_w), dim=-1) + self._body_com_state_w.timestamp = self._sim_timestamp + + return self._body_com_state_w.data + + @property + def body_com_acc_w(self): + """Acceleration of all bodies center of mass ``[lin_acc, ang_acc]``. + Shape is (num_instances, num_bodies, 6). + + All values are relative to the world. + """ + if self._body_com_acc_w.timestamp < self._sim_timestamp: + # read data from simulation and set the buffer data and timestamp + self._body_com_acc_w.data = self._root_physx_view.get_link_accelerations() + self._body_com_acc_w.timestamp = self._sim_timestamp + + return self._body_com_acc_w.data + + @property + def body_com_pose_b(self) -> torch.Tensor: + """Center of mass pose ``[pos, quat]`` of all bodies in their respective body's link frames. + Shape is (num_instances, 1, 7). + + This quantity is the pose of the center of mass frame of the rigid body relative to the body's link frame. + The orientation is provided in (w, x, y, z) format. + """ + if self._body_com_pose_b.timestamp < self._sim_timestamp: + # read data from simulation + pose = self._root_physx_view.get_coms().to(self.device) + pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz") + # set the buffer data and timestamp + self._body_com_pose_b.data = pose + self._body_com_pose_b.timestamp = self._sim_timestamp + + return self._body_com_pose_b.data + + @property + def body_incoming_joint_wrench_b(self) -> torch.Tensor: + """Joint reaction wrench applied from body parent to child body in parent body frame. + + Shape is (num_instances, num_bodies, 6). All body reaction wrenches are provided including the root body to the + world of an articulation. + + For more information on joint wrenches, please check the`PhysX documentation `__ + and the underlying `PhysX Tensor API `__ . + """ + + if self._body_incoming_joint_wrench_b.timestamp < self._sim_timestamp: + self._body_incoming_joint_wrench_b.data = self._root_physx_view.get_link_incoming_joint_force() + self._body_incoming_joint_wrench_b.time_stamp = self._sim_timestamp + return self._body_incoming_joint_wrench_b.data + + ## + # Joint state properties. + ## + + @property + def joint_pos(self): + """Joint positions of all joints. Shape is (num_instances, num_joints).""" + if self._joint_pos.timestamp < self._sim_timestamp: + # read data from simulation and set the buffer data and timestamp + self._joint_pos.data = self._root_physx_view.get_dof_positions() + self._joint_pos.timestamp = self._sim_timestamp + return self._joint_pos.data + + @property + def joint_vel(self): + """Joint velocities of all joints. Shape is (num_instances, num_joints).""" + if self._joint_vel.timestamp < self._sim_timestamp: + # read data from simulation and set the buffer data and timestamp + self._joint_vel.data = self._root_physx_view.get_dof_velocities() + self._joint_vel.timestamp = self._sim_timestamp + return self._joint_vel.data + + @property + def joint_acc(self): + """Joint acceleration of all joints. Shape is (num_instances, num_joints).""" + if self._joint_acc.timestamp < self._sim_timestamp: + # note: we use finite differencing to compute acceleration + time_elapsed = self._sim_timestamp - self._joint_acc.timestamp + self._joint_acc.data = (self.joint_vel - self._previous_joint_vel) / time_elapsed + self._joint_acc.timestamp = self._sim_timestamp + # update the previous joint velocity + self._previous_joint_vel[:] = self.joint_vel + return self._joint_acc.data + + ## + # Derived Properties. + ## + + @property + def projected_gravity_b(self): + """Projection of the gravity direction on base frame. Shape is (num_instances, 3).""" + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W) + + @property + def heading_w(self): + """Yaw heading of the base frame (in radians). Shape is (num_instances,). + + Note: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B) + return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + + @property + def root_link_lin_vel_b(self) -> torch.Tensor: + """Root link linear velocity in base frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the articulation root's actor frame with respect to the + its actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w) + + @property + def root_link_ang_vel_b(self) -> torch.Tensor: + """Root link angular velocity in base world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the articulation root's actor frame with respect to the + its actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) + + @property + def root_com_lin_vel_b(self) -> torch.Tensor: + """Root center of mass linear velocity in base frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the articulation root's center of mass frame with respect to the + its actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) + + @property + def root_com_ang_vel_b(self) -> torch.Tensor: + """Root center of mass angular velocity in base world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the articulation root's center of mass frame with respect to the + its actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) + + ## + # Sliced properties. + ## + + @property + def root_link_pos_w(self) -> torch.Tensor: + """Root link position in simulation world frame. Shape is (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + return self.root_link_pose_w[:, :3] + + @property + def root_link_quat_w(self) -> torch.Tensor: + """Root link orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body. + """ + return self.root_link_pose_w[:, 3:7] + + @property + def root_link_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's actor frame relative to the world. + """ + return self.root_link_vel_w[:, :3] + + @property + def root_link_ang_vel_w(self) -> torch.Tensor: + """Root link angular velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. + """ + return self.root_link_vel_w[:, 3:6] + + @property + def root_com_pos_w(self) -> torch.Tensor: + """Root center of mass position in simulation world frame. Shape is (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + return self.root_com_pose_w[:, :3] + + @property + def root_com_quat_w(self) -> torch.Tensor: + """Root center of mass orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body relative to the world. + """ + return self.root_com_pose_w[:, 3:7] + + @property + def root_com_lin_vel_w(self) -> torch.Tensor: + """Root center of mass linear velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. + """ + return self.root_com_vel_w[:, :3] + + @property + def root_com_ang_vel_w(self) -> torch.Tensor: + """Root center of mass angular velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. + """ + return self.root_com_vel_w[:, 3:6] + + @property + def body_link_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the position of the articulation bodies' actor frame relative to the world. + """ + return self.body_link_pose_w[..., :3] + + @property + def body_link_quat_w(self) -> torch.Tensor: + """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 4). + + This quantity is the orientation of the articulation bodies' actor frame relative to the world. + """ + return self.body_link_pose_w[..., 3:7] + + @property + def body_link_lin_vel_w(self) -> torch.Tensor: + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the linear velocity of the articulation bodies' center of mass frame relative to the world. + """ + return self.body_link_vel_w[..., :3] + + @property + def body_link_ang_vel_w(self) -> torch.Tensor: + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the angular velocity of the articulation bodies' center of mass frame relative to the world. + """ + return self.body_link_vel_w[..., 3:6] + + @property + def body_com_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the position of the articulation bodies' actor frame. + """ + return self.body_com_pose_w[..., :3] + + @property + def body_com_quat_w(self) -> torch.Tensor: + """Orientation (w, x, y, z) of the principle axis of inertia of all bodies in simulation world frame. + Shape is (num_instances, num_bodies, 4). + + This quantity is the orientation of the articulation bodies' actor frame. + """ + return self.body_com_pose_w[..., 3:7] + + @property + def body_com_lin_vel_w(self) -> torch.Tensor: + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the linear velocity of the articulation bodies' center of mass frame. + """ + return self.body_com_vel_w[..., :3] + + @property + def body_com_ang_vel_w(self) -> torch.Tensor: + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the angular velocity of the articulation bodies' center of mass frame. + """ + return self.body_com_vel_w[..., 3:6] + + @property + def body_com_lin_acc_w(self) -> torch.Tensor: + """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the linear acceleration of the articulation bodies' center of mass frame. + """ + return self.body_com_acc_w[..., :3] + + @property + def body_com_ang_acc_w(self) -> torch.Tensor: + """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, num_bodies, 3). + + This quantity is the angular acceleration of the articulation bodies' center of mass frame. + """ + return self.body_com_acc_w[..., 3:6] + + @property + def body_com_pos_b(self) -> torch.Tensor: + """Center of mass position of all of the bodies in their respective link frames. + Shape is (num_instances, num_bodies, 3). + + This quantity is the center of mass location relative to its body'slink frame. + """ + return self.body_com_pose_b[..., :3] + + @property + def body_com_quat_b(self) -> torch.Tensor: + """Orientation (w, x, y, z) of the principle axis of inertia of all of the bodies in their + respective link frames. Shape is (num_instances, num_bodies, 4). + + This quantity is the orientation of the principles axes of inertia relative to its body's link frame. + """ + return self.body_com_pose_b[..., 3:7] + + ## + # Backward compatibility. + ## + + @property + def root_pose_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pose_w`.""" + return self.root_link_pose_w + + @property + def root_pos_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pos_w`.""" + return self.root_link_pos_w + + @property + def root_quat_w(self) -> torch.Tensor: + """Same as :attr:`root_link_quat_w`.""" + return self.root_link_quat_w + + @property + def root_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_vel_w`.""" + return self.root_com_vel_w + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_w`.""" + return self.root_com_lin_vel_w + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_w`.""" + return self.root_com_ang_vel_w + + @property + def root_lin_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_b`.""" + return self.root_com_lin_vel_b + + @property + def root_ang_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_b`.""" + return self.root_com_ang_vel_b + + @property + def body_pose_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pose_w`.""" + return self.body_link_pose_w + + @property + def body_pos_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pos_w`.""" + return self.body_link_pos_w + + @property + def body_quat_w(self) -> torch.Tensor: + """Same as :attr:`body_link_quat_w`.""" + return self.body_link_quat_w + + @property + def body_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_vel_w`.""" + return self.body_com_vel_w + + @property + def body_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_vel_w`.""" + return self.body_com_lin_vel_w + + @property + def body_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_vel_w`.""" + return self.body_com_ang_vel_w + + @property + def body_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_acc_w`.""" + return self.body_com_acc_w + + @property + def body_lin_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_acc_w`.""" + return self.body_com_lin_acc_w + + @property + def body_ang_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_acc_w`.""" + return self.body_com_ang_acc_w + + @property + def com_pos_b(self) -> torch.Tensor: + """Same as :attr:`body_com_pos_b`.""" + return self.body_com_pos_b + + @property + def com_quat_b(self) -> torch.Tensor: + """Same as :attr:`body_com_quat_b`.""" + return self.body_com_quat_b + + @property + def joint_limits(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`joint_pos_limits` instead.""" + omni.log.warn( + "The `joint_limits` property will be deprecated in a future release. Please use `joint_pos_limits` instead." + ) + return self.joint_pos_limits + + @property + def default_joint_limits(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`default_joint_pos_limits` instead.""" + omni.log.warn( + "The `default_joint_limits` property will be deprecated in a future release. Please use" + " `default_joint_pos_limits` instead." + ) + return self.default_joint_pos_limits + + @property + def joint_velocity_limits(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`joint_vel_limits` instead.""" + omni.log.warn( + "The `joint_velocity_limits` property will be deprecated in a future release. Please use" + " `joint_vel_limits` instead." + ) + return self.joint_vel_limits + + @property + def joint_friction(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`joint_friction_coeff` instead.""" + omni.log.warn( + "The `joint_friction` property will be deprecated in a future release. Please use" + " `joint_friction_coeff` instead." + ) + return self.joint_friction_coeff + + @property + def default_joint_friction(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`default_joint_friction_coeff` instead.""" + omni.log.warn( + "The `default_joint_friction` property will be deprecated in a future release. Please use" + " `default_joint_friction_coeff` instead." + ) + return self.default_joint_friction_coeff + + @property + def fixed_tendon_limit(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`fixed_tendon_pos_limits` instead.""" + omni.log.warn( + "The `fixed_tendon_limit` property will be deprecated in a future release. Please use" + " `fixed_tendon_pos_limits` instead." + ) + return self.fixed_tendon_pos_limits + + @property + def default_fixed_tendon_limit(self) -> torch.Tensor: + """Deprecated property. Please use :attr:`default_fixed_tendon_pos_limits` instead.""" + omni.log.warn( + "The `default_fixed_tendon_limit` property will be deprecated in a future release. Please use" + " `default_fixed_tendon_pos_limits` instead." + ) + return self.default_fixed_tendon_pos_limits diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base.py new file mode 100644 index 00000000000..c4aff04c87a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import builtins +import inspect +import re +import torch +import weakref +from abc import ABC, abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import isaacsim.core.utils.prims as prim_utils +import omni.kit.app +import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager + +import isaaclab.sim as sim_utils + +if TYPE_CHECKING: + from .asset_base_cfg import AssetBaseCfg + + +class AssetBase(ABC): + """The base interface class for assets. + + An asset corresponds to any physics-enabled object that can be spawned in the simulation. These include + rigid objects, articulated objects, deformable objects etc. The core functionality of an asset is to + provide a set of buffers that can be used to interact with the simulator. The buffers are updated + by the asset class and can be written into the simulator using the their respective ``write`` methods. + This allows a convenient way to perform post-processing operations on the buffers before writing them + into the simulator and obtaining the corresponding simulation results. + + The class handles both the spawning of the asset into the USD stage as well as initialization of necessary + physics handles to interact with the asset. Upon construction of the asset instance, the prim corresponding + to the asset is spawned into the USD stage if the spawn configuration is not None. The spawn configuration + is defined in the :attr:`AssetBaseCfg.spawn` attribute. In case the configured :attr:`AssetBaseCfg.prim_path` + is an expression, then the prim is spawned at all the matching paths. Otherwise, a single prim is spawned + at the configured path. For more information on the spawn configuration, see the + :mod:`isaaclab.sim.spawners` module. + + Unlike Isaac Sim interface, where one usually needs to call the + :meth:`isaacsim.core.prims.XFormPrim.initialize` method to initialize the PhysX handles, the asset + class automatically initializes and invalidates the PhysX handles when the stage is played/stopped. This + is done by registering callbacks for the stage play/stop events. + + Additionally, the class registers a callback for debug visualization of the asset if a debug visualization + is implemented in the asset class. This can be enabled by setting the :attr:`AssetBaseCfg.debug_vis` attribute + to True. The debug visualization is implemented through the :meth:`_set_debug_vis_impl` and + :meth:`_debug_vis_callback` methods. + """ + + def __init__(self, cfg: AssetBaseCfg): + """Initialize the asset base. + + Args: + cfg: The configuration class for the asset. + + Raises: + RuntimeError: If no prims found at input prim path or prim path expression. + """ + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg.copy() + # flag for whether the asset is initialized + self._is_initialized = False + + # check if base asset path is valid + # note: currently the spawner does not work if there is a regex pattern in the leaf + # For example, if the prim path is "/World/Robot_[1,2]" since the spawner will not + # know which prim to spawn. This is a limitation of the spawner and not the asset. + asset_path = self.cfg.prim_path.split("/")[-1] + asset_path_is_regex = re.match(r"^[a-zA-Z0-9/_]+$", asset_path) is None + # spawn the asset + if self.cfg.spawn is not None and not asset_path_is_regex: + self.cfg.spawn.func( + self.cfg.prim_path, + self.cfg.spawn, + translation=self.cfg.init_state.pos, + orientation=self.cfg.init_state.rot, + ) + # check that spawn was successful + matching_prims = sim_utils.find_matching_prims(self.cfg.prim_path) + if len(matching_prims) == 0: + raise RuntimeError(f"Could not find prim with path {self.cfg.prim_path}.") + + # note: Use weakref on all callbacks to ensure that this object can be deleted when its destructor is called. + # add callbacks for stage play/stop + # The order is set to 10 which is arbitrary but should be lower priority than the default order of 0 + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj=weakref.proxy(self): obj._initialize_callback(event), + order=10, + ) + self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj=weakref.proxy(self): obj._invalidate_initialize_callback(event), + order=10, + ) + self._prim_deletion_callback_id = SimulationManager.register_callback( + self._on_prim_deletion, event=IsaacEvents.PRIM_DELETION + ) + # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) + self._debug_vis_handle = None + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + + def __del__(self): + """Unsubscribe from the callbacks.""" + # clear events handles + self._clear_callbacks() + + """ + Properties + """ + + @property + def is_initialized(self) -> bool: + """Whether the asset is initialized. + + Returns True if the asset is initialized, False otherwise. + """ + return self._is_initialized + + @property + @abstractmethod + def num_instances(self) -> int: + """Number of instances of the asset. + + This is equal to the number of asset instances per environment multiplied by the number of environments. + """ + return NotImplementedError + + @property + def device(self) -> str: + """Memory device for computation.""" + return self._device + + @property + @abstractmethod + def data(self) -> Any: + """Data related to the asset.""" + return NotImplementedError + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the asset has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_debug_vis_impl) + return "NotImplementedError" not in source_code + + """ + Operations. + """ + + def set_visibility(self, visible: bool, env_ids: Sequence[int] | None = None): + """Set the visibility of the prims corresponding to the asset. + + This operation affects the visibility of the prims corresponding to the asset in the USD stage. + It is useful for toggling the visibility of the asset in the simulator. For instance, one can + hide the asset when it is not being used to reduce the rendering overhead. + + Note: + This operation uses the PXR API to set the visibility of the prims. Thus, the operation + may have an overhead if the number of prims is large. + + Args: + visible: Whether to make the prims visible or not. + env_ids: The indices of the object to set visibility. Defaults to None (all instances). + """ + # resolve the environment ids + if env_ids is None: + env_ids = range(len(self._prims)) + elif isinstance(env_ids, torch.Tensor): + env_ids = env_ids.detach().cpu().tolist() + + # obtain the prims corresponding to the asset + # note: we only want to find the prims once since this is a costly operation + if not hasattr(self, "_prims"): + self._prims = sim_utils.find_matching_prims(self.cfg.prim_path) + + # iterate over the environment ids + for env_id in env_ids: + prim_utils.set_prim_visibility(self._prims[env_id], visible) + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Sets whether to visualize the asset data. + + Args: + debug_vis: Whether to visualize the asset data. + + Returns: + Whether the debug visualization was successfully set. False if the asset + does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + @abstractmethod + def reset(self, env_ids: Sequence[int] | None = None): + """Resets all internal buffers of selected environments. + + Args: + env_ids: The indices of the object to reset. Defaults to None (all instances). + """ + raise NotImplementedError + + @abstractmethod + def write_data_to_sim(self): + """Writes data to the simulator.""" + raise NotImplementedError + + @abstractmethod + def update(self, dt: float): + """Update the internal buffers. + + The time step ``dt`` is used to compute numerical derivatives of quantities such as joint + accelerations which are not provided by the simulator. + + Args: + dt: The amount of time passed from last ``update`` call. + """ + raise NotImplementedError + + """ + Implementation specific. + """ + + @abstractmethod + def _initialize_impl(self): + """Initializes the PhysX handles and internal buffers.""" + raise NotImplementedError + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + def _debug_vis_callback(self, event): + """Callback for debug visualization. + + This function calls the visualization objects and sets the data to visualize into them. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + """ + Internal simulation callbacks. + """ + + def _initialize_callback(self, event): + """Initializes the scene elements. + + Note: + PhysX handles are only enabled once the simulator starts playing. Hence, this function needs to be + called whenever the simulator "plays" from a "stop" state. + """ + if not self._is_initialized: + # obtain simulation related information + self._backend = SimulationManager.get_backend() + self._device = SimulationManager.get_physics_sim_device() + # initialize the asset + try: + self._initialize_impl() + except Exception as e: + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e + # set flag + self._is_initialized = True + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + self._is_initialized = False + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + def _on_prim_deletion(self, prim_path: str) -> None: + """Invalidates and deletes the callbacks when the prim is deleted. + + Args: + prim_path: The path to the prim that is being deleted. + + Note: + This function is called when the prim is deleted. + """ + if prim_path == "/": + self._clear_callbacks() + return + result = re.match( + pattern="^" + "/".join(self.cfg.prim_path.split("/")[: prim_path.count("/") + 1]) + "$", string=prim_path + ) + if result: + self._clear_callbacks() + + def _clear_callbacks(self) -> None: + """Clears the callbacks.""" + if self._prim_deletion_callback_id: + SimulationManager.deregister_callback(self._prim_deletion_callback_id) + self._prim_deletion_callback_id = None + if self._initialize_handle: + self._initialize_handle.unsubscribe() + self._initialize_handle = None + if self._invalidate_initialize_handle: + self._invalidate_initialize_handle.unsubscribe() + self._invalidate_initialize_handle = None + # clear debug visualization + if self._debug_vis_handle: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base_cfg.py new file mode 100644 index 00000000000..8f47d6e5578 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/asset_base_cfg.py @@ -0,0 +1,82 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim import SpawnerCfg +from isaaclab.utils import configclass + +from .asset_base import AssetBase + + +@configclass +class AssetBaseCfg: + """The base configuration class for an asset's parameters. + + Please see the :class:`AssetBase` class for more information on the asset class. + """ + + @configclass + class InitialStateCfg: + """Initial state of the asset. + + This defines the default initial state of the asset when it is spawned into the simulation, as + well as the default state when the simulation is reset. + + After parsing the initial state, the asset class stores this information in the :attr:`data` + attribute of the asset class. This can then be accessed by the user to modify the state of the asset + during the simulation, for example, at resets. + """ + + # root position + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) of the root in simulation world frame. + Defaults to (1.0, 0.0, 0.0, 0.0). + """ + + class_type: type[AssetBase] = None + """The associated asset class. Defaults to None, which means that the asset will be spawned + but cannot be interacted with via the asset class. + + The class should inherit from :class:`isaaclab.assets.asset_base.AssetBase`. + """ + + prim_path: str = MISSING + """Prim path (or expression) to the asset. + + .. note:: + The expression can contain the environment namespace regex ``{ENV_REGEX_NS}`` which + will be replaced with the environment namespace. + + Example: ``{ENV_REGEX_NS}/Robot`` will be replaced with ``/World/envs/env_.*/Robot``. + """ + + spawn: SpawnerCfg | None = None + """Spawn configuration for the asset. Defaults to None. + + If None, then no prims are spawned by the asset class. Instead, it is assumed that the + asset is already present in the scene. + """ + + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the rigid object. Defaults to identity pose.""" + + collision_group: Literal[0, -1] = 0 + """Collision group of the asset. Defaults to ``0``. + + * ``-1``: global collision group (collides with all assets in the scene). + * ``0``: local collision group (collides with other assets in the same environment). + """ + + debug_vis: bool = False + """Whether to enable debug visualization for the asset. Defaults to ``False``.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/__init__.py new file mode 100644 index 00000000000..aedcf265b95 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for deformable object assets.""" + +from .deformable_object import DeformableObject +from .deformable_object_cfg import DeformableObjectCfg +from .deformable_object_data import DeformableObjectData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object.py new file mode 100644 index 00000000000..afd9790d4de --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object.py @@ -0,0 +1,416 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log +import omni.physics.tensors.impl.api as physx +from isaacsim.core.simulation_manager import SimulationManager +from pxr import PhysxSchema, UsdShade + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.markers import VisualizationMarkers + +from ..asset_base import AssetBase +from .deformable_object_data import DeformableObjectData + +if TYPE_CHECKING: + from .deformable_object_cfg import DeformableObjectCfg + + +class DeformableObject(AssetBase): + """A deformable object asset class. + + Deformable objects are assets that can be deformed in the simulation. They are typically used for + soft bodies, such as stuffed animals and food items. + + Unlike rigid object assets, deformable objects have a more complex structure and require additional + handling for simulation. The simulation of deformable objects follows a finite element approach, where + the object is discretized into a mesh of nodes and elements. The nodes are connected by elements, which + define the material properties of the object. The nodes can be moved and deformed, and the elements + respond to these changes. + + The state of a deformable object comprises of its nodal positions and velocities, and not the object's root + position and orientation. The nodal positions and velocities are in the simulation frame. + + Soft bodies can be `partially kinematic`_, where some nodes are driven by kinematic targets, and the rest are + simulated. The kinematic targets are the desired positions of the nodes, and the simulation drives the nodes + towards these targets. This is useful for partial control of the object, such as moving a stuffed animal's + head while the rest of the body is simulated. + + .. attention:: + This class is experimental and subject to change due to changes on the underlying PhysX API on which + it depends. We will try to maintain backward compatibility as much as possible but some changes may be + necessary. + + .. _partially kinematic: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/SoftBodies.html#kinematic-soft-bodies + """ + + cfg: DeformableObjectCfg + """Configuration instance for the deformable object.""" + + def __init__(self, cfg: DeformableObjectCfg): + """Initialize the deformable object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + def data(self) -> DeformableObjectData: + return self._data + + @property + def num_instances(self) -> int: + return self.root_physx_view.count + + @property + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single deformable body. + """ + return 1 + + @property + def root_physx_view(self) -> physx.SoftBodyView: + """Deformable body view for the asset (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._root_physx_view + + @property + def material_physx_view(self) -> physx.SoftBodyMaterialView | None: + """Deformable material view for the asset (PhysX). + + This view is optional and may not be available if the material is not bound to the deformable body. + If the material is not available, then the material properties will be set to default values. + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._material_physx_view + + @property + def max_sim_elements_per_body(self) -> int: + """The maximum number of simulation mesh elements per deformable body.""" + return self.root_physx_view.max_sim_elements_per_body + + @property + def max_collision_elements_per_body(self) -> int: + """The maximum number of collision mesh elements per deformable body.""" + return self.root_physx_view.max_elements_per_body + + @property + def max_sim_vertices_per_body(self) -> int: + """The maximum number of simulation mesh vertices per deformable body.""" + return self.root_physx_view.max_sim_vertices_per_body + + @property + def max_collision_vertices_per_body(self) -> int: + """The maximum number of collision mesh vertices per deformable body.""" + return self.root_physx_view.max_vertices_per_body + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # Think: Should we reset the kinematic targets when resetting the object? + # This is not done in the current implementation. We assume users will reset the kinematic targets. + pass + + def write_data_to_sim(self): + pass + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Write to simulation. + """ + + def write_nodal_state_to_sim(self, nodal_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the nodal state over selected environment indices into the simulation. + + The nodal state comprises of the nodal positions and velocities. Since these are nodes, the velocity only has + a translational component. All the quantities are in the simulation frame. + + Args: + nodal_state: Nodal state in simulation frame. + Shape is (len(env_ids), max_sim_vertices_per_body, 6). + env_ids: Environment indices. If None, then all indices are used. + """ + # set into simulation + self.write_nodal_pos_to_sim(nodal_state[..., :3], env_ids=env_ids) + self.write_nodal_velocity_to_sim(nodal_state[..., 3:], env_ids=env_ids) + + def write_nodal_pos_to_sim(self, nodal_pos: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the nodal positions over selected environment indices into the simulation. + + The nodal position comprises of individual nodal positions of the simulation mesh for the deformable body. + The positions are in the simulation frame. + + Args: + nodal_pos: Nodal positions in simulation frame. + Shape is (len(env_ids), max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.nodal_pos_w[env_ids] = nodal_pos.clone() + # set into simulation + self.root_physx_view.set_sim_nodal_positions(self._data.nodal_pos_w, indices=physx_env_ids) + + def write_nodal_velocity_to_sim(self, nodal_vel: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the nodal velocity over selected environment indices into the simulation. + + The nodal velocity comprises of individual nodal velocities of the simulation mesh for the deformable + body. Since these are nodes, the velocity only has a translational component. The velocities are in the + simulation frame. + + Args: + nodal_vel: Nodal velocities in simulation frame. + Shape is (len(env_ids), max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.nodal_vel_w[env_ids] = nodal_vel.clone() + # set into simulation + self.root_physx_view.set_sim_nodal_velocities(self._data.nodal_vel_w, indices=physx_env_ids) + + def write_nodal_kinematic_target_to_sim(self, targets: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the kinematic targets of the simulation mesh for the deformable bodies indicated by the indices. + + The kinematic targets comprise of individual nodal positions of the simulation mesh for the deformable body + and a flag indicating whether the node is kinematically driven or not. The positions are in the simulation frame. + + Note: + The flag is set to 0.0 for kinematically driven nodes and 1.0 for free nodes. + + Args: + targets: The kinematic targets comprising of nodal positions and flags. + Shape is (len(env_ids), max_sim_vertices_per_body, 4). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + # store into internal buffers + self._data.nodal_kinematic_target[env_ids] = targets.clone() + # set into simulation + self.root_physx_view.set_sim_kinematic_targets(self._data.nodal_kinematic_target, indices=physx_env_ids) + + """ + Operations - Helper. + """ + + def transform_nodal_pos( + self, nodal_pos: torch.tensor, pos: torch.Tensor | None = None, quat: torch.Tensor | None = None + ) -> torch.Tensor: + """Transform the nodal positions based on the pose transformation. + + This function computes the transformation of the nodal positions based on the pose transformation. + It multiplies the nodal positions with the rotation matrix of the pose and adds the translation. + Internally, it calls the :meth:`isaaclab.utils.math.transform_points` function. + + Args: + nodal_pos: The nodal positions in the simulation frame. Shape is (N, max_sim_vertices_per_body, 3). + pos: The position transformation. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + quat: The orientation transformation as quaternion (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + The transformed nodal positions. Shape is (N, max_sim_vertices_per_body, 3). + """ + # offset the nodal positions to center them around the origin + mean_nodal_pos = nodal_pos.mean(dim=1, keepdim=True) + nodal_pos = nodal_pos - mean_nodal_pos + # transform the nodal positions based on the pose around the origin + return math_utils.transform_points(nodal_pos, pos, quat) + mean_nodal_pos + + """ + Internal helper. + """ + + def _initialize_impl(self): + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # obtain the first prim in the regex expression (all others are assumed to be a copy of this) + template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if template_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") + template_prim_path = template_prim.GetPath().pathString + + # find deformable root prims + root_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, predicate=lambda prim: prim.HasAPI(PhysxSchema.PhysxDeformableBodyAPI) + ) + if len(root_prims) == 0: + raise RuntimeError( + f"Failed to find a deformable body when resolving '{self.cfg.prim_path}'." + " Please ensure that the prim has 'PhysxSchema.PhysxDeformableBodyAPI' applied." + ) + if len(root_prims) > 1: + raise RuntimeError( + f"Failed to find a single deformable body when resolving '{self.cfg.prim_path}'." + f" Found multiple '{root_prims}' under '{template_prim_path}'." + " Please ensure that there is only one deformable body in the prim path tree." + ) + # we only need the first one from the list + root_prim = root_prims[0] + + # find deformable material prims + material_prim = None + # obtain material prim from the root prim + # note: here we assume that all the root prims have their material prims at similar paths + # and we only need to find the first one. This may not be the case for all scenarios. + # However, the checks in that case get cumbersome and are not included here. + if root_prim.HasAPI(UsdShade.MaterialBindingAPI): + # check the materials that are bound with the purpose 'physics' + material_paths = UsdShade.MaterialBindingAPI(root_prim).GetDirectBindingRel("physics").GetTargets() + # iterate through targets and find the deformable body material + if len(material_paths) > 0: + for mat_path in material_paths: + mat_prim = root_prim.GetStage().GetPrimAtPath(mat_path) + if mat_prim.HasAPI(PhysxSchema.PhysxDeformableBodyMaterialAPI): + material_prim = mat_prim + break + if material_prim is None: + omni.log.info( + f"Failed to find a deformable material binding for '{root_prim.GetPath().pathString}'." + " The material properties will be set to default values and are not modifiable at runtime." + " If you want to modify the material properties, please ensure that the material is bound" + " to the deformable body." + ) + + # resolve root path back into regex expression + # -- root prim expression + root_prim_path = root_prim.GetPath().pathString + root_prim_path_expr = self.cfg.prim_path + root_prim_path[len(template_prim_path) :] + # -- object view + self._root_physx_view = self._physics_sim_view.create_soft_body_view(root_prim_path_expr.replace(".*", "*")) + + # Return if the asset is not found + if self._root_physx_view._backend is None: + raise RuntimeError(f"Failed to create deformable body at: {self.cfg.prim_path}. Please check PhysX logs.") + + # resolve material path back into regex expression + if material_prim is not None: + # -- material prim expression + material_prim_path = material_prim.GetPath().pathString + # check if the material prim is under the template prim + # if not then we are assuming that the single material prim is used for all the deformable bodies + if template_prim_path in material_prim_path: + material_prim_path_expr = self.cfg.prim_path + material_prim_path[len(template_prim_path) :] + else: + material_prim_path_expr = material_prim_path + # -- material view + self._material_physx_view = self._physics_sim_view.create_soft_body_material_view( + material_prim_path_expr.replace(".*", "*") + ) + else: + self._material_physx_view = None + + # log information about the deformable body + omni.log.info(f"Deformable body initialized at: {root_prim_path_expr}") + omni.log.info(f"Number of instances: {self.num_instances}") + omni.log.info(f"Number of bodies: {self.num_bodies}") + if self._material_physx_view is not None: + omni.log.info(f"Deformable material initialized at: {material_prim_path_expr}") + omni.log.info(f"Number of instances: {self._material_physx_view.count}") + else: + omni.log.info("No deformable material found. Material properties will be set to default values.") + + # container for data access + self._data = DeformableObjectData(self.root_physx_view, self.device) + + # create buffers + self._create_buffers() + # update the deformable body data + self.update(0.0) + + def _create_buffers(self): + """Create buffers for storing data.""" + # constants + self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + + # default state + # we use the initial nodal positions at spawn time as the default state + # note: these are all in the simulation frame + nodal_positions = self.root_physx_view.get_sim_nodal_positions() + nodal_velocities = torch.zeros_like(nodal_positions) + self._data.default_nodal_state_w = torch.cat((nodal_positions, nodal_velocities), dim=-1) + + # kinematic targets + self._data.nodal_kinematic_target = self.root_physx_view.get_sim_kinematic_targets() + # set all nodes as non-kinematic targets by default + self._data.nodal_kinematic_target[..., -1] = 1.0 + + """ + Internal simulation callbacks. + """ + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + if not hasattr(self, "target_visualizer"): + self.target_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self.target_visualizer.set_visibility(True) + else: + if hasattr(self, "target_visualizer"): + self.target_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # check where to visualize + targets_enabled = self.data.nodal_kinematic_target[:, :, 3] == 0.0 + num_enabled = int(torch.sum(targets_enabled).item()) + # get positions if any targets are enabled + if num_enabled == 0: + # create a marker below the ground + positions = torch.tensor([[0.0, 0.0, -10.0]], device=self.device) + else: + positions = self.data.nodal_kinematic_target[targets_enabled][..., :3] + # show target visualizer + self.target_visualizer.visualize(positions) + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + self._root_physx_view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_cfg.py new file mode 100644 index 00000000000..a685f722291 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_cfg.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import DEFORMABLE_TARGET_MARKER_CFG +from isaaclab.utils import configclass + +from ..asset_base_cfg import AssetBaseCfg +from .deformable_object import DeformableObject + + +@configclass +class DeformableObjectCfg(AssetBaseCfg): + """Configuration parameters for a deformable object.""" + + class_type: type = DeformableObject + + visualizer_cfg: VisualizationMarkersCfg = DEFORMABLE_TARGET_MARKER_CFG.replace( + prim_path="/Visuals/DeformableTarget" + ) + """The configuration object for the visualization markers. Defaults to DEFORMABLE_TARGET_MARKER_CFG. + + Note: + This attribute is only used when debug visualization is enabled. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_data.py new file mode 100644 index 00000000000..08a54baec6e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/deformable_object/deformable_object_data.py @@ -0,0 +1,240 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +import weakref + +import omni.physics.tensors.impl.api as physx + +import isaaclab.utils.math as math_utils +from isaaclab.utils.buffers import TimestampedBuffer + + +class DeformableObjectData: + """Data container for a deformable object. + + This class contains the data for a deformable object in the simulation. The data includes the nodal states of + the root deformable body in the object. The data is stored in the simulation world frame unless otherwise specified. + + A deformable object in PhysX uses two tetrahedral meshes to represent the object: + + 1. **Simulation mesh**: This mesh is used for the simulation and is the one that is deformed by the solver. + 2. **Collision mesh**: This mesh only needs to match the surface of the simulation mesh and is used for + collision detection. + + The APIs exposed provides the data for both the simulation and collision meshes. These are specified + by the `sim` and `collision` prefixes in the property names. + + The data is lazily updated, meaning that the data is only updated when it is accessed. This is useful + when the data is expensive to compute or retrieve. The data is updated when the timestamp of the buffer + is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. + """ + + def __init__(self, root_physx_view: physx.SoftBodyView, device: str): + """Initializes the deformable object data. + + Args: + root_physx_view: The root deformable body view of the object. + device: The device used for processing. + """ + # Set the parameters + self.device = device + # Set the root deformable body view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.SoftBodyView = weakref.proxy(root_physx_view) + + # Set initial time stamp + self._sim_timestamp = 0.0 + + # Initialize the lazy buffers. + # -- node state in simulation world frame + self._nodal_pos_w = TimestampedBuffer() + self._nodal_vel_w = TimestampedBuffer() + self._nodal_state_w = TimestampedBuffer() + # -- mesh element-wise rotations + self._sim_element_quat_w = TimestampedBuffer() + self._collision_element_quat_w = TimestampedBuffer() + # -- mesh element-wise deformation gradients + self._sim_element_deform_gradient_w = TimestampedBuffer() + self._collision_element_deform_gradient_w = TimestampedBuffer() + # -- mesh element-wise stresses + self._sim_element_stress_w = TimestampedBuffer() + self._collision_element_stress_w = TimestampedBuffer() + + def update(self, dt: float): + """Updates the data for the deformable object. + + Args: + dt: The time step for the update. This must be a positive value. + """ + # update the simulation timestamp + self._sim_timestamp += dt + + ## + # Defaults. + ## + + default_nodal_state_w: torch.Tensor = None + """Default nodal state ``[nodal_pos, nodal_vel]`` in simulation world frame. + Shape is (num_instances, max_sim_vertices_per_body, 6). + """ + + ## + # Kinematic commands + ## + + nodal_kinematic_target: torch.Tensor = None + """Simulation mesh kinematic targets for the deformable bodies. + Shape is (num_instances, max_sim_vertices_per_body, 4). + + The kinematic targets are used to drive the simulation mesh vertices to the target positions. + The targets are stored as (x, y, z, is_not_kinematic) where "is_not_kinematic" is a binary + flag indicating whether the vertex is kinematic or not. The flag is set to 0 for kinematic vertices + and 1 for non-kinematic vertices. + """ + + ## + # Properties. + ## + + @property + def nodal_pos_w(self): + """Nodal positions in simulation world frame. Shape is (num_instances, max_sim_vertices_per_body, 3).""" + if self._nodal_pos_w.timestamp < self._sim_timestamp: + self._nodal_pos_w.data = self._root_physx_view.get_sim_nodal_positions() + self._nodal_pos_w.timestamp = self._sim_timestamp + return self._nodal_pos_w.data + + @property + def nodal_vel_w(self): + """Nodal velocities in simulation world frame. Shape is (num_instances, max_sim_vertices_per_body, 3).""" + if self._nodal_vel_w.timestamp < self._sim_timestamp: + self._nodal_vel_w.data = self._root_physx_view.get_sim_nodal_velocities() + self._nodal_vel_w.timestamp = self._sim_timestamp + return self._nodal_vel_w.data + + @property + def nodal_state_w(self): + """Nodal state ``[nodal_pos, nodal_vel]`` in simulation world frame. + Shape is (num_instances, max_sim_vertices_per_body, 6). + """ + if self._nodal_state_w.timestamp < self._sim_timestamp: + self._nodal_state_w.data = torch.cat((self.nodal_pos_w, self.nodal_vel_w), dim=-1) + self._nodal_state_w.timestamp = self._sim_timestamp + return self._nodal_state_w.data + + @property + def sim_element_quat_w(self): + """Simulation mesh element-wise rotations as quaternions for the deformable bodies in simulation world frame. + Shape is (num_instances, max_sim_elements_per_body, 4). + + The rotations are stored as quaternions in the order (w, x, y, z). + """ + if self._sim_element_quat_w.timestamp < self._sim_timestamp: + # convert from xyzw to wxyz + quats = self._root_physx_view.get_sim_element_rotations().view(self._root_physx_view.count, -1, 4) + quats = math_utils.convert_quat(quats, to="wxyz") + # set the buffer data and timestamp + self._sim_element_quat_w.data = quats + self._sim_element_quat_w.timestamp = self._sim_timestamp + return self._sim_element_quat_w.data + + @property + def collision_element_quat_w(self): + """Collision mesh element-wise rotations as quaternions for the deformable bodies in simulation world frame. + Shape is (num_instances, max_collision_elements_per_body, 4). + + The rotations are stored as quaternions in the order (w, x, y, z). + """ + if self._collision_element_quat_w.timestamp < self._sim_timestamp: + # convert from xyzw to wxyz + quats = self._root_physx_view.get_element_rotations().view(self._root_physx_view.count, -1, 4) + quats = math_utils.convert_quat(quats, to="wxyz") + # set the buffer data and timestamp + self._collision_element_quat_w.data = quats + self._collision_element_quat_w.timestamp = self._sim_timestamp + return self._collision_element_quat_w.data + + @property + def sim_element_deform_gradient_w(self): + """Simulation mesh element-wise second-order deformation gradient tensors for the deformable bodies + in simulation world frame. Shape is (num_instances, max_sim_elements_per_body, 3, 3). + """ + if self._sim_element_deform_gradient_w.timestamp < self._sim_timestamp: + # set the buffer data and timestamp + self._sim_element_deform_gradient_w.data = ( + self._root_physx_view.get_sim_element_deformation_gradients().view( + self._root_physx_view.count, -1, 3, 3 + ) + ) + self._sim_element_deform_gradient_w.timestamp = self._sim_timestamp + return self._sim_element_deform_gradient_w.data + + @property + def collision_element_deform_gradient_w(self): + """Collision mesh element-wise second-order deformation gradient tensors for the deformable bodies + in simulation world frame. Shape is (num_instances, max_collision_elements_per_body, 3, 3). + """ + if self._collision_element_deform_gradient_w.timestamp < self._sim_timestamp: + # set the buffer data and timestamp + self._collision_element_deform_gradient_w.data = ( + self._root_physx_view.get_element_deformation_gradients().view(self._root_physx_view.count, -1, 3, 3) + ) + self._collision_element_deform_gradient_w.timestamp = self._sim_timestamp + return self._collision_element_deform_gradient_w.data + + @property + def sim_element_stress_w(self): + """Simulation mesh element-wise second-order Cauchy stress tensors for the deformable bodies + in simulation world frame. Shape is (num_instances, max_sim_elements_per_body, 3, 3). + """ + if self._sim_element_stress_w.timestamp < self._sim_timestamp: + # set the buffer data and timestamp + self._sim_element_stress_w.data = self._root_physx_view.get_sim_element_stresses().view( + self._root_physx_view.count, -1, 3, 3 + ) + self._sim_element_stress_w.timestamp = self._sim_timestamp + return self._sim_element_stress_w.data + + @property + def collision_element_stress_w(self): + """Collision mesh element-wise second-order Cauchy stress tensors for the deformable bodies + in simulation world frame. Shape is (num_instances, max_collision_elements_per_body, 3, 3). + """ + if self._collision_element_stress_w.timestamp < self._sim_timestamp: + # set the buffer data and timestamp + self._collision_element_stress_w.data = self._root_physx_view.get_element_stresses().view( + self._root_physx_view.count, -1, 3, 3 + ) + self._collision_element_stress_w.timestamp = self._sim_timestamp + return self._collision_element_stress_w.data + + ## + # Derived properties. + ## + + @property + def root_pos_w(self) -> torch.Tensor: + """Root position from nodal positions of the simulation mesh for the deformable bodies in simulation world frame. + Shape is (num_instances, 3). + + This quantity is computed as the mean of the nodal positions. + """ + return self.nodal_pos_w.mean(dim=1) + + @property + def root_vel_w(self) -> torch.Tensor: + """Root velocity from vertex velocities for the deformable bodies in simulation world frame. + Shape is (num_instances, 3). + + This quantity is computed as the mean of the nodal velocities. + """ + return self.nodal_vel_w.mean(dim=1) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/__init__.py new file mode 100644 index 00000000000..5b660dde3bb --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for rigid object assets.""" + +from .rigid_object import RigidObject +from .rigid_object_cfg import RigidObjectCfg +from .rigid_object_data import RigidObjectData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object.py new file mode 100644 index 00000000000..44edf6ff1f1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object.py @@ -0,0 +1,512 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log +import omni.physics.tensors.impl.api as physx +from isaacsim.core.simulation_manager import SimulationManager +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils + +from ..asset_base import AssetBase +from .rigid_object_data import RigidObjectData + +if TYPE_CHECKING: + from .rigid_object_cfg import RigidObjectCfg + + +class RigidObject(AssetBase): + """A rigid object asset class. + + Rigid objects are assets comprising of rigid bodies. They can be used to represent dynamic objects + such as boxes, spheres, etc. A rigid body is described by its pose, velocity and mass distribution. + + For an asset to be considered a rigid object, the root prim of the asset must have the `USD RigidBodyAPI`_ + applied to it. This API is used to define the simulation properties of the rigid body. On playing the + simulation, the physics engine will automatically register the rigid body and create a corresponding + rigid body handle. This handle can be accessed using the :attr:`root_physx_view` attribute. + + .. note:: + + For users familiar with Isaac Sim, the PhysX view class API is not the exactly same as Isaac Sim view + class API. Similar to Isaac Lab, Isaac Sim wraps around the PhysX view API. However, as of now (2023.1 release), + we see a large difference in initializing the view classes in Isaac Sim. This is because the view classes + in Isaac Sim perform additional USD-related operations which are slow and also not required. + + .. _`USD RigidBodyAPI`: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html + """ + + cfg: RigidObjectCfg + """Configuration instance for the rigid object.""" + + def __init__(self, cfg: RigidObjectCfg): + """Initialize the rigid object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + def data(self) -> RigidObjectData: + return self._data + + @property + def num_instances(self) -> int: + return self.root_physx_view.count + + @property + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single rigid body. + """ + return 1 + + @property + def body_names(self) -> list[str]: + """Ordered names of bodies in the rigid object.""" + prim_paths = self.root_physx_view.prim_paths[: self.num_bodies] + return [path.split("/")[-1] for path in prim_paths] + + @property + def root_physx_view(self) -> physx.RigidBodyView: + """Rigid body view for the asset (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._root_physx_view + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # resolve all indices + if env_ids is None: + env_ids = slice(None) + # reset external wrench + self._external_force_b[env_ids] = 0.0 + self._external_torque_b[env_ids] = 0.0 + + def write_data_to_sim(self): + """Write external wrench to the simulation. + + Note: + We write external wrench to the simulation here since this function is called before the simulation step. + This ensures that the external wrench is applied at every simulation step. + """ + # write external wrench + if self.has_external_wrench: + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self._external_force_b.view(-1, 3), + torque_data=self._external_torque_b.view(-1, 3), + position_data=None, + indices=self._ALL_INDICES, + is_global=False, + ) + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the rigid body based on the name keys. + + Please check the :meth:`isaaclab.utils.string_utils.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self.body_names, preserve_order) + + """ + Operations - Write to simulation. + """ + + def write_root_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_com_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_com_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_link_state_to_sim(self, root_state: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link state over selected environment indices into the simulation. + + The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + root_state: Root state in simulation frame. Shape is (len(env_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_state[:, :7], env_ids=env_ids) + self.write_root_link_velocity_to_sim(root_state[:, 7:], env_ids=env_ids) + + def write_root_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + root_pose: Root link poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim(root_pose, env_ids=env_ids) + + def write_root_link_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + root_pose: Root link poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_link_pose_w[env_ids] = root_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_link_state_w.data is not None: + self._data.root_link_state_w[env_ids, :7] = self._data.root_link_pose_w[env_ids] + if self._data._root_state_w.data is not None: + self._data.root_state_w[env_ids, :7] = self._data.root_link_pose_w[env_ids] + + # convert root quaternion from wxyz to xyzw + root_poses_xyzw = self._data.root_link_pose_w.clone() + root_poses_xyzw[:, 3:] = math_utils.convert_quat(root_poses_xyzw[:, 3:], to="xyzw") + + # set into simulation + self.root_physx_view.set_transforms(root_poses_xyzw, indices=physx_env_ids) + + def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + The orientation is the orientation of the principle axes of inertia. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + if env_ids is None: + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids + + # set into internal buffers + self._data.root_com_pose_w[local_env_ids] = root_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_com_state_w.data is not None: + self._data.root_com_state_w[local_env_ids, :7] = self._data.root_com_pose_w[local_env_ids] + + # get CoM pose in link frame + com_pos_b = self.data.body_com_pos_b[local_env_ids, 0, :] + com_quat_b = self.data.body_com_quat_b[local_env_ids, 0, :] + # transform input CoM pose to link frame + root_link_pos, root_link_quat = math_utils.combine_frame_transforms( + root_pose[..., :3], + root_pose[..., 3:7], + math_utils.quat_apply(math_utils.quat_inv(com_quat_b), -com_pos_b), + math_utils.quat_inv(com_quat_b), + ) + root_link_pose = torch.cat((root_link_pos, root_link_quat), dim=-1) + + # write transformed pose in link frame to sim + self.write_root_link_pose_to_sim(root_link_pose, env_ids=env_ids) + + def write_root_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's center of mass rather than the roots frame. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_com_velocity_to_sim(root_velocity=root_velocity, env_ids=env_ids) + + def write_root_com_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's center of mass rather than the roots frame. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + physx_env_ids = env_ids + if env_ids is None: + env_ids = slice(None) + physx_env_ids = self._ALL_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.root_com_vel_w[env_ids] = root_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_com_state_w.data is not None: + self._data.root_com_state_w[env_ids, 7:] = self._data.root_com_vel_w[env_ids] + if self._data._root_state_w.data is not None: + self._data.root_state_w[env_ids, 7:] = self._data.root_com_vel_w[env_ids] + # make the acceleration zero to prevent reporting old values + self._data.body_com_acc_w[env_ids] = 0.0 + + # set into simulation + self.root_physx_view.set_velocities(self._data.root_com_vel_w, indices=physx_env_ids) + + def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids: Sequence[int] | None = None): + """Set the root link velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the root's frame rather than the roots center of mass. + + Args: + root_velocity: Root frame velocities in simulation world frame. Shape is (len(env_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + """ + # resolve all indices + if env_ids is None: + local_env_ids = slice(env_ids) + else: + local_env_ids = env_ids + + # set into internal buffers + self._data.root_link_vel_w[local_env_ids] = root_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._root_link_state_w.data is not None: + self._data.root_link_state_w[local_env_ids, 7:] = self._data.root_link_vel_w[local_env_ids] + + # get CoM pose in link frame + quat = self.data.root_link_quat_w[local_env_ids] + com_pos_b = self.data.body_com_pos_b[local_env_ids, 0, :] + # transform input velocity to center of mass frame + root_com_velocity = root_velocity.clone() + root_com_velocity[:, :3] += torch.linalg.cross( + root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1 + ) + + # write transformed velocity in CoM frame to sim + self.write_root_com_velocity_to_sim(root_com_velocity, env_ids=env_ids) + + """ + Operations - Setters. + """ + + def set_external_force_and_torque( + self, + forces: torch.Tensor, + torques: torch.Tensor, + body_ids: Sequence[int] | slice | None = None, + env_ids: Sequence[int] | None = None, + ): + """Set external force and torque to apply on the asset's bodies in their local frame. + + For many applications, we want to keep the applied external force on rigid bodies constant over a period of + time (for instance, during the policy control). This function allows us to store the external force and torque + into buffers which are then applied to the simulation at every step. + + .. caution:: + If the function is called with empty forces and torques, then this function disables the application + of external wrench to the simulation. + + .. code-block:: python + + # example of disabling external wrench + asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3)) + + .. note:: + This function does not apply the external wrench to the simulation. It only fills the buffers with + the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function + right before the simulation step. + + Args: + forces: External forces in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + torques: External torques in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). + body_ids: Body indices to apply external wrench to. Defaults to None (all bodies). + env_ids: Environment indices to apply external wrench to. Defaults to None (all instances). + """ + if forces.any() or torques.any(): + self.has_external_wrench = True + else: + self.has_external_wrench = False + # to be safe, explicitly set value to zero + forces = torques = 0.0 + + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = slice(None) + # -- body_ids + if body_ids is None: + body_ids = slice(None) + # broadcast env_ids if needed to allow double indexing + if env_ids != slice(None) and body_ids != slice(None): + env_ids = env_ids[:, None] + # set into internal buffers + self._external_force_b[env_ids, body_ids] = forces + self._external_torque_b[env_ids, body_ids] = torques + + """ + Internal helper. + """ + + def _initialize_impl(self): + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # obtain the first prim in the regex expression (all others are assumed to be a copy of this) + template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if template_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") + template_prim_path = template_prim.GetPath().pathString + + # find rigid root prims + root_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI) + ) + if len(root_prims) == 0: + raise RuntimeError( + f"Failed to find a rigid body when resolving '{self.cfg.prim_path}'." + " Please ensure that the prim has 'USD RigidBodyAPI' applied." + ) + if len(root_prims) > 1: + raise RuntimeError( + f"Failed to find a single rigid body when resolving '{self.cfg.prim_path}'." + f" Found multiple '{root_prims}' under '{template_prim_path}'." + " Please ensure that there is only one rigid body in the prim path tree." + ) + + articulation_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + ) + if len(articulation_prims) != 0: + if articulation_prims[0].GetAttribute("physxArticulation:articulationEnabled").Get(): + raise RuntimeError( + f"Found an articulation root when resolving '{self.cfg.prim_path}' for rigid objects. These are" + f" located at: '{articulation_prims}' under '{template_prim_path}'. Please disable the articulation" + " root in the USD or from code by setting the parameter" + " 'ArticulationRootPropertiesCfg.articulation_enabled' to False in the spawn configuration." + ) + + # resolve root prim back into regex expression + root_prim_path = root_prims[0].GetPath().pathString + root_prim_path_expr = self.cfg.prim_path + root_prim_path[len(template_prim_path) :] + # -- object view + self._root_physx_view = self._physics_sim_view.create_rigid_body_view(root_prim_path_expr.replace(".*", "*")) + + # check if the rigid body was created + if self._root_physx_view._backend is None: + raise RuntimeError(f"Failed to create rigid body at: {self.cfg.prim_path}. Please check PhysX logs.") + + # log information about the rigid body + omni.log.info(f"Rigid body initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.") + omni.log.info(f"Number of instances: {self.num_instances}") + omni.log.info(f"Number of bodies: {self.num_bodies}") + omni.log.info(f"Body names: {self.body_names}") + + # container for data access + self._data = RigidObjectData(self.root_physx_view, self.device) + + # create buffers + self._create_buffers() + # process configuration + self._process_cfg() + # update the rigid body data + self.update(0.0) + + def _create_buffers(self): + """Create buffers for storing data.""" + # constants + self._ALL_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + + # external forces and torques + self.has_external_wrench = False + self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device) + self._external_torque_b = torch.zeros_like(self._external_force_b) + + # set information about rigid body into data + self._data.body_names = self.body_names + self._data.default_mass = self.root_physx_view.get_masses().clone() + self._data.default_inertia = self.root_physx_view.get_inertias().clone() + + def _process_cfg(self): + """Post processing of configuration parameters.""" + # default state + # -- root state + # note: we cast to tuple to avoid torch/numpy type mismatch. + default_root_state = ( + tuple(self.cfg.init_state.pos) + + tuple(self.cfg.init_state.rot) + + tuple(self.cfg.init_state.lin_vel) + + tuple(self.cfg.init_state.ang_vel) + ) + default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device) + self._data.default_root_state = default_root_state.repeat(self.num_instances, 1) + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._root_physx_view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_cfg.py new file mode 100644 index 00000000000..c77b5a81548 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from ..asset_base_cfg import AssetBaseCfg +from .rigid_object import RigidObject + + +@configclass +class RigidObjectCfg(AssetBaseCfg): + """Configuration parameters for a rigid object.""" + + @configclass + class InitialStateCfg(AssetBaseCfg.InitialStateCfg): + """Initial state of the rigid body.""" + + lin_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Linear velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + ang_vel: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Angular velocity of the root in simulation world frame. Defaults to (0.0, 0.0, 0.0).""" + + ## + # Initialize configurations. + ## + + class_type: type = RigidObject + + init_state: InitialStateCfg = InitialStateCfg() + """Initial state of the rigid object. Defaults to identity pose with zero velocity.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_data.py new file mode 100644 index 00000000000..24b433493d3 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object/rigid_object_data.py @@ -0,0 +1,656 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +import weakref + +import omni.physics.tensors.impl.api as physx + +import isaaclab.utils.math as math_utils +from isaaclab.utils.buffers import TimestampedBuffer + + +class RigidObjectData: + """Data container for a rigid object. + + This class contains the data for a rigid object in the simulation. The data includes the state of + the root rigid body and the state of all the bodies in the object. The data is stored in the simulation + world frame unless otherwise specified. + + For a rigid body, there are two frames of reference that are used: + + - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim + with the rigid body schema. + - Center of mass frame: The frame of reference of the center of mass of the rigid body. + + Depending on the settings of the simulation, the actor frame and the center of mass frame may be the same. + This needs to be taken into account when interpreting the data. + + The data is lazily updated, meaning that the data is only updated when it is accessed. This is useful + when the data is expensive to compute or retrieve. The data is updated when the timestamp of the buffer + is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. + """ + + def __init__(self, root_physx_view: physx.RigidBodyView, device: str): + """Initializes the rigid object data. + + Args: + root_physx_view: The root rigid body view. + device: The device used for processing. + """ + # Set the parameters + self.device = device + # Set the root rigid body view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.RigidBodyView = weakref.proxy(root_physx_view) + + # Set initial time stamp + self._sim_timestamp = 0.0 + + # Obtain global physics sim view + physics_sim_view = physx.create_simulation_view("torch") + physics_sim_view.set_subspace_roots("/") + gravity = physics_sim_view.get_gravity() + # Convert to direction vector + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + + # Initialize constants + self.GRAVITY_VEC_W = gravity_dir.repeat(self._root_physx_view.count, 1) + self.FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self._root_physx_view.count, 1) + + # Initialize the lazy buffers. + # -- link frame w.r.t. world frame + self._root_link_pose_w = TimestampedBuffer() + self._root_link_vel_w = TimestampedBuffer() + # -- com frame w.r.t. link frame + self._body_com_pose_b = TimestampedBuffer() + # -- com frame w.r.t. world frame + self._root_com_pose_w = TimestampedBuffer() + self._root_com_vel_w = TimestampedBuffer() + self._body_com_acc_w = TimestampedBuffer() + # -- combined state (these are cached as they concatenate) + self._root_state_w = TimestampedBuffer() + self._root_link_state_w = TimestampedBuffer() + self._root_com_state_w = TimestampedBuffer() + + def update(self, dt: float): + """Updates the data for the rigid object. + + Args: + dt: The time step for the update. This must be a positive value. + """ + # update the simulation timestamp + self._sim_timestamp += dt + + ## + # Names. + ## + + body_names: list[str] = None + """Body names in the order parsed by the simulation view.""" + + ## + # Defaults. + ## + + default_root_state: torch.Tensor = None + """Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. Shape is (num_instances, 13). + + The position and quaternion are of the rigid body's actor frame. Meanwhile, the linear and angular velocities are + of the center of mass frame. + """ + + default_mass: torch.Tensor = None + """Default mass read from the simulation. Shape is (num_instances, 1).""" + + default_inertia: torch.Tensor = None + """Default inertia tensor read from the simulation. Shape is (num_instances, 9). + + The inertia is the inertia tensor relative to the center of mass frame. The values are stored in + the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + """ + + ## + # Root state properties. + ## + + @property + def root_link_pose_w(self) -> torch.Tensor: + """Root link pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 7). + + This quantity is the pose of the actor frame of the root rigid body relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._root_link_pose_w.timestamp < self._sim_timestamp: + # read data from simulation + pose = self._root_physx_view.get_transforms().clone() + pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") + # set the buffer data and timestamp + self._root_link_pose_w.data = pose + self._root_link_pose_w.timestamp = self._sim_timestamp + + return self._root_link_pose_w.data + + @property + def root_link_vel_w(self) -> torch.Tensor: + """Root link velocity ``[lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 6). + + This quantity contains the linear and angular velocities of the actor frame of the root + rigid body relative to the world. + """ + if self._root_link_vel_w.timestamp < self._sim_timestamp: + # read the CoM velocity + vel = self.root_com_vel_w.clone() + # adjust linear velocity to link from center of mass + vel[:, :3] += torch.linalg.cross( + vel[:, 3:], math_utils.quat_apply(self.root_link_quat_w, -self.body_com_pos_b[:, 0]), dim=-1 + ) + # set the buffer data and timestamp + self._root_link_vel_w.data = vel + self._root_link_vel_w.timestamp = self._sim_timestamp + + return self._root_link_vel_w.data + + @property + def root_com_pose_w(self) -> torch.Tensor: + """Root center of mass pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 7). + + This quantity is the pose of the center of mass frame of the root rigid body relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + if self._root_com_pose_w.timestamp < self._sim_timestamp: + # apply local transform to center of mass frame + pos, quat = math_utils.combine_frame_transforms( + self.root_link_pos_w, self.root_link_quat_w, self.body_com_pos_b[:, 0], self.body_com_quat_b[:, 0] + ) + # set the buffer data and timestamp + self._root_com_pose_w.data = torch.cat((pos, quat), dim=-1) + self._root_com_pose_w.timestamp = self._sim_timestamp + + return self._root_com_pose_w.data + + @property + def root_com_vel_w(self) -> torch.Tensor: + """Root center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 6). + + This quantity contains the linear and angular velocities of the root rigid body's center of mass frame + relative to the world. + """ + if self._root_com_vel_w.timestamp < self._sim_timestamp: + self._root_com_vel_w.data = self._root_physx_view.get_velocities() + self._root_com_vel_w.timestamp = self._sim_timestamp + + return self._root_com_vel_w.data + + @property + def root_state_w(self) -> torch.Tensor: + """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + + The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular + velocities are of the rigid body's center of mass frame. + """ + if self._root_state_w.timestamp < self._sim_timestamp: + self._root_state_w.data = torch.cat((self.root_link_pose_w, self.root_com_vel_w), dim=-1) + self._root_state_w.timestamp = self._sim_timestamp + + return self._root_state_w.data + + @property + def root_link_state_w(self) -> torch.Tensor: + """Root state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 13). + + The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the + world. The orientation is provided in (w, x, y, z) format. + """ + if self._root_link_state_w.timestamp < self._sim_timestamp: + self._root_link_state_w.data = torch.cat((self.root_link_pose_w, self.root_link_vel_w), dim=-1) + self._root_link_state_w.timestamp = self._sim_timestamp + + return self._root_link_state_w.data + + @property + def root_com_state_w(self) -> torch.Tensor: + """Root center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, 13). + + The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame + relative to the world. Center of mass frame is the orientation principle axes of inertia. + """ + if self._root_com_state_w.timestamp < self._sim_timestamp: + self._root_com_state_w.data = torch.cat((self.root_com_pose_w, self.root_com_vel_w), dim=-1) + self._root_com_state_w.timestamp = self._sim_timestamp + + return self._root_com_state_w.data + + ## + # Body state properties. + ## + + @property + def body_link_pose_w(self) -> torch.Tensor: + """Body link pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 1, 7). + + This quantity is the pose of the actor frame of the rigid body relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + return self.root_link_pose_w.view(-1, 1, 7) + + @property + def body_link_vel_w(self) -> torch.Tensor: + """Body link velocity ``[lin_vel, ang_vel]`` in simulation world frame. Shape is (num_instances, 1, 6). + + This quantity contains the linear and angular velocities of the actor frame of the root + rigid body relative to the world. + """ + return self.root_link_vel_w.view(-1, 1, 6) + + @property + def body_com_pose_w(self) -> torch.Tensor: + """Body center of mass pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, 1, 7). + + This quantity is the pose of the center of mass frame of the rigid body relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + return self.root_com_pose_w.view(-1, 1, 7) + + @property + def body_com_vel_w(self) -> torch.Tensor: + """Body center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, 1, 6). + + This quantity contains the linear and angular velocities of the root rigid body's center of mass frame + relative to the world. + """ + return self.root_com_vel_w.view(-1, 1, 6) + + @property + def body_state_w(self) -> torch.Tensor: + """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame. + Shape is (num_instances, 1, 13). + + The position and orientation are of the rigid bodies' actor frame. Meanwhile, the linear and angular + velocities are of the rigid bodies' center of mass frame. + """ + return self.root_state_w.view(-1, 1, 13) + + @property + def body_link_state_w(self) -> torch.Tensor: + """State of all bodies ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, 1, 13). + + The position, quaternion, and linear/angular velocity are of the body's link frame relative to the world. + The orientation is provided in (w, x, y, z) format. + """ + return self.root_link_state_w.view(-1, 1, 13) + + @property + def body_com_state_w(self) -> torch.Tensor: + """State of all bodies ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_bodies, 13). + + The position, quaternion, and linear/angular velocity are of the body's center of mass frame relative to the + world. Center of mass frame is assumed to be the same orientation as the link rather than the orientation of the + principle inertia. The orientation is provided in (w, x, y, z) format. + """ + return self.root_com_state_w.view(-1, 1, 13) + + @property + def body_com_acc_w(self) -> torch.Tensor: + """Acceleration of all bodies ``[lin_acc, ang_acc]`` in the simulation world frame. + Shape is (num_instances, 1, 6). + + This quantity is the acceleration of the rigid bodies' center of mass frame relative to the world. + """ + if self._body_com_acc_w.timestamp < self._sim_timestamp: + self._body_com_acc_w.data = self._root_physx_view.get_accelerations().unsqueeze(1) + self._body_com_acc_w.timestamp = self._sim_timestamp + + return self._body_com_acc_w.data + + @property + def body_com_pose_b(self) -> torch.Tensor: + """Center of mass pose ``[pos, quat]`` of all bodies in their respective body's link frames. + Shape is (num_instances, 1, 7). + + This quantity is the pose of the center of mass frame of the rigid body relative to the body's link frame. + The orientation is provided in (w, x, y, z) format. + """ + if self._body_com_pose_b.timestamp < self._sim_timestamp: + # read data from simulation + pose = self._root_physx_view.get_coms().to(self.device) + pose[:, 3:7] = math_utils.convert_quat(pose[:, 3:7], to="wxyz") + # set the buffer data and timestamp + self._body_com_pose_b.data = pose.view(-1, 1, 7) + self._body_com_pose_b.timestamp = self._sim_timestamp + + return self._body_com_pose_b.data + + ## + # Derived Properties. + ## + + @property + def projected_gravity_b(self) -> torch.Tensor: + """Projection of the gravity direction on base frame. Shape is (num_instances, 3).""" + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W) + + @property + def heading_w(self) -> torch.Tensor: + """Yaw heading of the base frame (in radians). Shape is (num_instances,). + + Note: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + forward_w = math_utils.quat_apply(self.root_link_quat_w, self.FORWARD_VEC_B) + return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + + @property + def root_link_lin_vel_b(self) -> torch.Tensor: + """Root link linear velocity in base frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the actor frame of the root rigid body frame with respect to the + rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w) + + @property + def root_link_ang_vel_b(self) -> torch.Tensor: + """Root link angular velocity in base world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the actor frame of the root rigid body frame with respect to the + rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w) + + @property + def root_com_lin_vel_b(self) -> torch.Tensor: + """Root center of mass linear velocity in base frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the + rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w) + + @property + def root_com_ang_vel_b(self) -> torch.Tensor: + """Root center of mass angular velocity in base world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the + rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_ang_vel_w) + + ## + # Sliced properties. + ## + + @property + def root_link_pos_w(self) -> torch.Tensor: + """Root link position in simulation world frame. Shape is (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + return self.root_link_pose_w[:, :3] + + @property + def root_link_quat_w(self) -> torch.Tensor: + """Root link orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body. + """ + return self.root_link_pose_w[:, 3:7] + + @property + def root_link_lin_vel_w(self) -> torch.Tensor: + """Root linear velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's actor frame relative to the world. + """ + return self.root_link_vel_w[:, :3] + + @property + def root_link_ang_vel_w(self) -> torch.Tensor: + """Root link angular velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. + """ + return self.root_link_vel_w[:, 3:6] + + @property + def root_com_pos_w(self) -> torch.Tensor: + """Root center of mass position in simulation world frame. Shape is (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + return self.root_com_pose_w[:, :3] + + @property + def root_com_quat_w(self) -> torch.Tensor: + """Root center of mass orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body relative to the world. + """ + return self.root_com_pose_w[:, 3:7] + + @property + def root_com_lin_vel_w(self) -> torch.Tensor: + """Root center of mass linear velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. + """ + return self.root_com_vel_w[:, :3] + + @property + def root_com_ang_vel_w(self) -> torch.Tensor: + """Root center of mass angular velocity in simulation world frame. Shape is (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. + """ + return self.root_com_vel_w[:, 3:6] + + @property + def body_link_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the position of the rigid bodies' actor frame relative to the world. + """ + return self.body_link_pose_w[..., :3] + + @property + def body_link_quat_w(self) -> torch.Tensor: + """Orientation (w, x, y, z) of all bodies in simulation world frame. Shape is (num_instances, 1, 4). + + This quantity is the orientation of the rigid bodies' actor frame relative to the world. + """ + return self.body_link_pose_w[..., 3:7] + + @property + def body_link_lin_vel_w(self) -> torch.Tensor: + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the linear velocity of the rigid bodies' center of mass frame relative to the world. + """ + return self.body_link_vel_w[..., :3] + + @property + def body_link_ang_vel_w(self) -> torch.Tensor: + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the angular velocity of the rigid bodies' center of mass frame relative to the world. + """ + return self.body_link_vel_w[..., 3:6] + + @property + def body_com_pos_w(self) -> torch.Tensor: + """Positions of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the position of the rigid bodies' actor frame. + """ + return self.body_com_pose_w[..., :3] + + @property + def body_com_quat_w(self) -> torch.Tensor: + """Orientation (w, x, y, z) of the principle axis of inertia of all bodies in simulation world frame. + + Shape is (num_instances, 1, 4). This quantity is the orientation of the rigid bodies' actor frame. + """ + return self.body_com_pose_w[..., 3:7] + + @property + def body_com_lin_vel_w(self) -> torch.Tensor: + """Linear velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the linear velocity of the rigid bodies' center of mass frame. + """ + return self.body_com_vel_w[..., :3] + + @property + def body_com_ang_vel_w(self) -> torch.Tensor: + """Angular velocity of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the angular velocity of the rigid bodies' center of mass frame. + """ + return self.body_com_vel_w[..., 3:6] + + @property + def body_com_lin_acc_w(self) -> torch.Tensor: + """Linear acceleration of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the linear acceleration of the rigid bodies' center of mass frame. + """ + return self.body_com_acc_w[..., :3] + + @property + def body_com_ang_acc_w(self) -> torch.Tensor: + """Angular acceleration of all bodies in simulation world frame. Shape is (num_instances, 1, 3). + + This quantity is the angular acceleration of the rigid bodies' center of mass frame. + """ + return self.body_com_acc_w[..., 3:6] + + @property + def body_com_pos_b(self) -> torch.Tensor: + """Center of mass position of all of the bodies in their respective link frames. + Shape is (num_instances, 1, 3). + + This quantity is the center of mass location relative to its body'slink frame. + """ + return self.body_com_pose_b[..., :3] + + @property + def body_com_quat_b(self) -> torch.Tensor: + """Orientation (w, x, y, z) of the principle axis of inertia of all of the bodies in their + respective link frames. Shape is (num_instances, 1, 4). + + This quantity is the orientation of the principles axes of inertia relative to its body's link frame. + """ + return self.body_com_pose_b[..., 3:7] + + ## + # Properties for backwards compatibility. + ## + + @property + def root_pose_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pose_w`.""" + return self.root_link_pose_w + + @property + def root_pos_w(self) -> torch.Tensor: + """Same as :attr:`root_link_pos_w`.""" + return self.root_link_pos_w + + @property + def root_quat_w(self) -> torch.Tensor: + """Same as :attr:`root_link_quat_w`.""" + return self.root_link_quat_w + + @property + def root_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_vel_w`.""" + return self.root_com_vel_w + + @property + def root_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_w`.""" + return self.root_com_lin_vel_w + + @property + def root_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_w`.""" + return self.root_com_ang_vel_w + + @property + def root_lin_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_lin_vel_b`.""" + return self.root_com_lin_vel_b + + @property + def root_ang_vel_b(self) -> torch.Tensor: + """Same as :attr:`root_com_ang_vel_b`.""" + return self.root_com_ang_vel_b + + @property + def body_pose_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pose_w`.""" + return self.body_link_pose_w + + @property + def body_pos_w(self) -> torch.Tensor: + """Same as :attr:`body_link_pos_w`.""" + return self.body_link_pos_w + + @property + def body_quat_w(self) -> torch.Tensor: + """Same as :attr:`body_link_quat_w`.""" + return self.body_link_quat_w + + @property + def body_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_vel_w`.""" + return self.body_com_vel_w + + @property + def body_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_vel_w`.""" + return self.body_com_lin_vel_w + + @property + def body_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_vel_w`.""" + return self.body_com_ang_vel_w + + @property + def body_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_acc_w`.""" + return self.body_com_acc_w + + @property + def body_lin_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_lin_acc_w`.""" + return self.body_com_lin_acc_w + + @property + def body_ang_acc_w(self) -> torch.Tensor: + """Same as :attr:`body_com_ang_acc_w`.""" + return self.body_com_ang_acc_w + + @property + def com_pos_b(self) -> torch.Tensor: + """Same as :attr:`body_com_pos_b`.""" + return self.body_com_pos_b + + @property + def com_quat_b(self) -> torch.Tensor: + """Same as :attr:`body_com_quat_b`.""" + return self.body_com_quat_b diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/__init__.py new file mode 100644 index 00000000000..cc42cfcfde8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for rigid object collection.""" + +from .rigid_object_collection import RigidObjectCollection +from .rigid_object_collection_cfg import RigidObjectCollectionCfg +from .rigid_object_collection_data import RigidObjectCollectionData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection.py new file mode 100644 index 00000000000..033572fe00c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection.py @@ -0,0 +1,714 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import re +import torch +import weakref +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.kit.app +import omni.log +import omni.physics.tensors.impl.api as physx +import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils + +from ..asset_base import AssetBase +from .rigid_object_collection_data import RigidObjectCollectionData + +if TYPE_CHECKING: + from .rigid_object_collection_cfg import RigidObjectCollectionCfg + + +class RigidObjectCollection(AssetBase): + """A rigid object collection class. + + This class represents a collection of rigid objects in the simulation, where the state of the + rigid objects can be accessed and modified using a batched ``(env_ids, object_ids)`` API. + + For each rigid body in the collection, the root prim of the asset must have the `USD RigidBodyAPI`_ + applied to it. This API is used to define the simulation properties of the rigid bodies. On playing the + simulation, the physics engine will automatically register the rigid bodies and create a corresponding + rigid body handle. This handle can be accessed using the :attr:`root_physx_view` attribute. + + Rigid objects in the collection are uniquely identified via the key of the dictionary + :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` in the + :class:`~isaaclab.assets.RigidObjectCollectionCfg` configuration class. + This differs from the :class:`~isaaclab.assets.RigidObject` class, where a rigid object is identified by + the name of the Xform where the `USD RigidBodyAPI`_ is applied. This would not be possible for the rigid + object collection since the :attr:`~isaaclab.assets.RigidObjectCollectionCfg.rigid_objects` dictionary + could contain the same rigid object multiple times, leading to ambiguity. + + .. _`USD RigidBodyAPI`: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html + """ + + cfg: RigidObjectCollectionCfg + """Configuration instance for the rigid object collection.""" + + def __init__(self, cfg: RigidObjectCollectionCfg): + """Initialize the rigid object collection. + + Args: + cfg: A configuration instance. + """ + # Note: We never call the parent constructor as it tries to call its own spawning which we don't want. + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg.copy() + # flag for whether the asset is initialized + self._is_initialized = False + self._prim_paths = [] + # spawn the rigid objects + for rigid_object_cfg in self.cfg.rigid_objects.values(): + # check if the rigid object path is valid + # note: currently the spawner does not work if there is a regex pattern in the leaf + # For example, if the prim path is "/World/Object_[1,2]" since the spawner will not + # know which prim to spawn. This is a limitation of the spawner and not the asset. + asset_path = rigid_object_cfg.prim_path.split("/")[-1] + asset_path_is_regex = re.match(r"^[a-zA-Z0-9/_]+$", asset_path) is None + # spawn the asset + if rigid_object_cfg.spawn is not None and not asset_path_is_regex: + rigid_object_cfg.spawn.func( + rigid_object_cfg.prim_path, + rigid_object_cfg.spawn, + translation=rigid_object_cfg.init_state.pos, + orientation=rigid_object_cfg.init_state.rot, + ) + # check that spawn was successful + matching_prims = sim_utils.find_matching_prims(rigid_object_cfg.prim_path) + if len(matching_prims) == 0: + raise RuntimeError(f"Could not find prim with path {rigid_object_cfg.prim_path}.") + self._prim_paths.append(rigid_object_cfg.prim_path) + # stores object names + self._object_names_list = [] + + # note: Use weakref on all callbacks to ensure that this object can be deleted when its destructor is called. + # add callbacks for stage play/stop + # The order is set to 10 which is arbitrary but should be lower priority than the default order of 0 + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj=weakref.proxy(self): obj._initialize_callback(event), + order=10, + ) + self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj=weakref.proxy(self): obj._invalidate_initialize_callback(event), + order=10, + ) + self._prim_deletion_callback_id = SimulationManager.register_callback( + self._on_prim_deletion, event=IsaacEvents.PRIM_DELETION + ) + self._debug_vis_handle = None + + """ + Properties + """ + + @property + def data(self) -> RigidObjectCollectionData: + return self._data + + @property + def num_instances(self) -> int: + """Number of instances of the collection.""" + return self.root_physx_view.count // self.num_objects + + @property + def num_objects(self) -> int: + """Number of objects in the collection. + + This corresponds to the distinct number of rigid bodies in the collection. + """ + return len(self.object_names) + + @property + def object_names(self) -> list[str]: + """Ordered names of objects in the rigid object collection.""" + return self._object_names_list + + @property + def root_physx_view(self) -> physx.RigidBodyView: + """Rigid body view for the rigid body collection (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._root_physx_view # type: ignore + + """ + Operations. + """ + + def reset(self, env_ids: torch.Tensor | None = None, object_ids: slice | torch.Tensor | None = None): + """Resets all internal buffers of selected environments and objects. + + Args: + env_ids: The indices of the object to reset. Defaults to None (all instances). + object_ids: The indices of the object to reset. Defaults to None (all objects). + """ + # resolve all indices + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + # reset external wrench + self._external_force_b[env_ids[:, None], object_ids] = 0.0 + self._external_torque_b[env_ids[:, None], object_ids] = 0.0 + + def write_data_to_sim(self): + """Write external wrench to the simulation. + + Note: + We write external wrench to the simulation here since this function is called before the simulation step. + This ensures that the external wrench is applied at every simulation step. + """ + # write external wrench + if self.has_external_wrench: + self.root_physx_view.apply_forces_and_torques_at_position( + force_data=self.reshape_data_to_view(self._external_force_b), + torque_data=self.reshape_data_to_view(self._external_torque_b), + position_data=None, + indices=self._env_obj_ids_to_view_ids(self._ALL_ENV_INDICES, self._ALL_OBJ_INDICES), + is_global=False, + ) + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_objects( + self, name_keys: str | Sequence[str], preserve_order: bool = False + ) -> tuple[torch.Tensor, list[str]]: + """Find objects in the collection based on the name keys. + + Please check the :meth:`isaaclab.utils.string_utils.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the object names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple containing the object indices and names. + """ + obj_ids, obj_names = string_utils.resolve_matching_names(name_keys, self.object_names, preserve_order) + return torch.tensor(obj_ids, device=self.device), obj_names + + """ + Operations - Write to simulation. + """ + + def write_object_state_to_sim( + self, + object_state: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object state over selected environment and object indices into the simulation. + + The object state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + object_state: Object state in simulation frame. Shape is (len(env_ids), len(object_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + self.write_object_link_pose_to_sim(object_state[..., :7], env_ids=env_ids, object_ids=object_ids) + self.write_object_com_velocity_to_sim(object_state[..., 7:], env_ids=env_ids, object_ids=object_ids) + + def write_object_com_state_to_sim( + self, + object_state: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object center of mass state over selected environment indices into the simulation. + + The object state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + object_state: Object state in simulation frame. Shape is (len(env_ids), len(object_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + self.write_object_com_pose_to_sim(object_state[..., :7], env_ids=env_ids, object_ids=object_ids) + self.write_object_com_velocity_to_sim(object_state[..., 7:], env_ids=env_ids, object_ids=object_ids) + + def write_object_link_state_to_sim( + self, + object_state: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object link state over selected environment indices into the simulation. + + The object state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear + and angular velocity. All the quantities are in the simulation frame. + + Args: + object_state: Object state in simulation frame. Shape is (len(env_ids), len(object_ids), 13). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + self.write_object_link_pose_to_sim(object_state[..., :7], env_ids=env_ids, object_ids=object_ids) + self.write_object_link_velocity_to_sim(object_state[..., 7:], env_ids=env_ids, object_ids=object_ids) + + def write_object_pose_to_sim( + self, + object_pose: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object pose over selected environment and object indices into the simulation. + + The object pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + object_pose: Object poses in simulation frame. Shape is (len(env_ids), len(object_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + self.write_object_link_pose_to_sim(object_pose, env_ids=env_ids, object_ids=object_ids) + + def write_object_link_pose_to_sim( + self, + object_pose: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object pose over selected environment and object indices into the simulation. + + The object pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + + Args: + object_pose: Object poses in simulation frame. Shape is (len(env_ids), len(object_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.object_link_pose_w[env_ids[:, None], object_ids] = object_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._object_link_state_w.data is not None: + self._data.object_link_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() + if self._data._object_state_w.data is not None: + self._data.object_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() + + # convert the quaternion from wxyz to xyzw + poses_xyzw = self._data.object_link_pose_w.clone() + poses_xyzw[..., 3:] = math_utils.convert_quat(poses_xyzw[..., 3:], to="xyzw") + + # set into simulation + view_ids = self._env_obj_ids_to_view_ids(env_ids, object_ids) + self.root_physx_view.set_transforms(self.reshape_data_to_view(poses_xyzw), indices=view_ids) + + def write_object_com_pose_to_sim( + self, + object_pose: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object center of mass pose over selected environment indices into the simulation. + + The object pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). + The orientation is the orientation of the principle axes of inertia. + + Args: + object_pose: Object poses in simulation frame. Shape is (len(env_ids), len(object_ids), 7). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + + # set into internal buffers + self._data.object_com_pose_w[env_ids[:, None], object_ids] = object_pose.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._object_com_state_w.data is not None: + self._data.object_com_state_w[env_ids[:, None], object_ids, :7] = object_pose.clone() + + # get CoM pose in link frame + com_pos_b = self.data.object_com_pos_b[env_ids[:, None], object_ids] + com_quat_b = self.data.object_com_quat_b[env_ids[:, None], object_ids] + # transform input CoM pose to link frame + object_link_pos, object_link_quat = math_utils.combine_frame_transforms( + object_pose[..., :3], + object_pose[..., 3:7], + math_utils.quat_apply(math_utils.quat_inv(com_quat_b), -com_pos_b), + math_utils.quat_inv(com_quat_b), + ) + + # write transformed pose in link frame to sim + object_link_pose = torch.cat((object_link_pos, object_link_quat), dim=-1) + self.write_object_link_pose_to_sim(object_link_pose, env_ids=env_ids, object_ids=object_ids) + + def write_object_velocity_to_sim( + self, + object_velocity: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object velocity over selected environment and object indices into the simulation. + + Args: + object_velocity: Object velocities in simulation frame. Shape is (len(env_ids), len(object_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + self.write_object_com_velocity_to_sim(object_velocity, env_ids=env_ids, object_ids=object_ids) + + def write_object_com_velocity_to_sim( + self, + object_velocity: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object center of mass velocity over selected environment and object indices into the simulation. + + Args: + object_velocity: Object velocities in simulation frame. Shape is (len(env_ids), len(object_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + + # note: we need to do this here since tensors are not set into simulation until step. + # set into internal buffers + self._data.object_com_vel_w[env_ids[:, None], object_ids] = object_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._object_com_state_w.data is not None: + self._data.object_com_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() + if self._data._object_state_w.data is not None: + self._data.object_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() + # make the acceleration zero to prevent reporting old values + self._data.object_com_acc_w[env_ids[:, None], object_ids] = 0.0 + + # set into simulation + view_ids = self._env_obj_ids_to_view_ids(env_ids, object_ids) + self.root_physx_view.set_velocities(self.reshape_data_to_view(self._data.object_com_vel_w), indices=view_ids) + + def write_object_link_velocity_to_sim( + self, + object_velocity: torch.Tensor, + env_ids: torch.Tensor | None = None, + object_ids: slice | torch.Tensor | None = None, + ): + """Set the object link velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + NOTE: This sets the velocity of the object's frame rather than the objects center of mass. + + Args: + object_velocity: Object velocities in simulation frame. Shape is (len(env_ids), len(object_ids), 6). + env_ids: Environment indices. If None, then all indices are used. + object_ids: Object indices. If None, then all indices are used. + """ + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + + # set into internal buffers + self._data.object_link_vel_w[env_ids[:, None], object_ids] = object_velocity.clone() + # update these buffers only if the user is using them. Otherwise this adds to overhead. + if self._data._object_link_state_w.data is not None: + self._data.object_link_state_w[env_ids[:, None], object_ids, 7:] = object_velocity.clone() + + # get CoM pose in link frame + quat = self.data.object_link_quat_w[env_ids[:, None], object_ids] + com_pos_b = self.data.object_com_pos_b[env_ids[:, None], object_ids] + # transform input velocity to center of mass frame + object_com_velocity = object_velocity.clone() + object_com_velocity[..., :3] += torch.linalg.cross( + object_com_velocity[..., 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1 + ) + + # write center of mass velocity to sim + self.write_object_com_velocity_to_sim(object_com_velocity, env_ids=env_ids, object_ids=object_ids) + + """ + Operations - Setters. + """ + + def set_external_force_and_torque( + self, + forces: torch.Tensor, + torques: torch.Tensor, + object_ids: slice | torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, + ): + """Set external force and torque to apply on the objects' bodies in their local frame. + + For many applications, we want to keep the applied external force on rigid bodies constant over a period of + time (for instance, during the policy control). This function allows us to store the external force and torque + into buffers which are then applied to the simulation at every step. + + .. caution:: + If the function is called with empty forces and torques, then this function disables the application + of external wrench to the simulation. + + .. code-block:: python + + # example of disabling external wrench + asset.set_external_force_and_torque(forces=torch.zeros(0, 0, 3), torques=torch.zeros(0, 0, 3)) + + .. note:: + This function does not apply the external wrench to the simulation. It only fills the buffers with + the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function + right before the simulation step. + + Args: + forces: External forces in bodies' local frame. Shape is (len(env_ids), len(object_ids), 3). + torques: External torques in bodies' local frame. Shape is (len(env_ids), len(object_ids), 3). + object_ids: Object indices to apply external wrench to. Defaults to None (all objects). + env_ids: Environment indices to apply external wrench to. Defaults to None (all instances). + """ + if forces.any() or torques.any(): + self.has_external_wrench = True + else: + self.has_external_wrench = False + # to be safe, explicitly set value to zero + forces = torques = 0.0 + + # resolve all indices + # -- env_ids + if env_ids is None: + env_ids = self._ALL_ENV_INDICES + # -- object_ids + if object_ids is None: + object_ids = self._ALL_OBJ_INDICES + # set into internal buffers + self._external_force_b[env_ids[:, None], object_ids] = forces + self._external_torque_b[env_ids[:, None], object_ids] = torques + + """ + Helper functions. + """ + + def reshape_view_to_data(self, data: torch.Tensor) -> torch.Tensor: + """Reshapes and arranges the data coming from the :attr:`root_physx_view` to + (num_instances, num_objects, data_dim). + + Args: + data: The data coming from the :attr:`root_physx_view`. Shape is (num_instances * num_objects, data_dim). + + Returns: + The reshaped data. Shape is (num_instances, num_objects, data_dim). + """ + return torch.einsum("ijk -> jik", data.reshape(self.num_objects, self.num_instances, -1)) + + def reshape_data_to_view(self, data: torch.Tensor) -> torch.Tensor: + """Reshapes and arranges the data to the be consistent with data from the :attr:`root_physx_view`. + + Args: + data: The data to be reshaped. Shape is (num_instances, num_objects, data_dim). + + Returns: + The reshaped data. Shape is (num_instances * num_objects, data_dim). + """ + return torch.einsum("ijk -> jik", data).reshape(self.num_objects * self.num_instances, *data.shape[2:]) + + """ + Internal helper. + """ + + def _initialize_impl(self): + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + root_prim_path_exprs = [] + for name, rigid_object_cfg in self.cfg.rigid_objects.items(): + # obtain the first prim in the regex expression (all others are assumed to be a copy of this) + template_prim = sim_utils.find_first_matching_prim(rigid_object_cfg.prim_path) + if template_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{rigid_object_cfg.prim_path}'.") + template_prim_path = template_prim.GetPath().pathString + + # find rigid root prims + root_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI) + ) + if len(root_prims) == 0: + raise RuntimeError( + f"Failed to find a rigid body when resolving '{rigid_object_cfg.prim_path}'." + " Please ensure that the prim has 'USD RigidBodyAPI' applied." + ) + if len(root_prims) > 1: + raise RuntimeError( + f"Failed to find a single rigid body when resolving '{rigid_object_cfg.prim_path}'." + f" Found multiple '{root_prims}' under '{template_prim_path}'." + " Please ensure that there is only one rigid body in the prim path tree." + ) + + # check that no rigid object has an articulation root API, which decreases simulation performance + articulation_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI) + ) + if len(articulation_prims) != 0: + if articulation_prims[0].GetAttribute("physxArticulation:articulationEnabled").Get(): + raise RuntimeError( + f"Found an articulation root when resolving '{rigid_object_cfg.prim_path}' in the rigid object" + f" collection. These are located at: '{articulation_prims}' under '{template_prim_path}'." + " Please disable the articulation root in the USD or from code by setting the parameter" + " 'ArticulationRootPropertiesCfg.articulation_enabled' to False in the spawn configuration." + ) + + # resolve root prim back into regex expression + root_prim_path = root_prims[0].GetPath().pathString + root_prim_path_expr = rigid_object_cfg.prim_path + root_prim_path[len(template_prim_path) :] + root_prim_path_exprs.append(root_prim_path_expr.replace(".*", "*")) + + self._object_names_list.append(name) + + # -- object view + self._root_physx_view = self._physics_sim_view.create_rigid_body_view(root_prim_path_exprs) + + # check if the rigid body was created + if self._root_physx_view._backend is None: + raise RuntimeError("Failed to create rigid body collection. Please check PhysX logs.") + + # log information about the rigid body + omni.log.info(f"Number of instances: {self.num_instances}") + omni.log.info(f"Number of distinct objects: {self.num_objects}") + omni.log.info(f"Object names: {self.object_names}") + + # container for data access + self._data = RigidObjectCollectionData(self.root_physx_view, self.num_objects, self.device) + + # create buffers + self._create_buffers() + # process configuration + self._process_cfg() + # update the rigid body data + self.update(0.0) + + def _create_buffers(self): + """Create buffers for storing data.""" + # constants + self._ALL_ENV_INDICES = torch.arange(self.num_instances, dtype=torch.long, device=self.device) + self._ALL_OBJ_INDICES = torch.arange(self.num_objects, dtype=torch.long, device=self.device) + + # external forces and torques + self.has_external_wrench = False + self._external_force_b = torch.zeros((self.num_instances, self.num_objects, 3), device=self.device) + self._external_torque_b = torch.zeros_like(self._external_force_b) + + # set information about rigid body into data + self._data.object_names = self.object_names + self._data.default_mass = self.reshape_view_to_data(self.root_physx_view.get_masses().clone()) + self._data.default_inertia = self.reshape_view_to_data(self.root_physx_view.get_inertias().clone()) + + def _process_cfg(self): + """Post processing of configuration parameters.""" + # default state + # -- object state + default_object_states = [] + for rigid_object_cfg in self.cfg.rigid_objects.values(): + default_object_state = ( + tuple(rigid_object_cfg.init_state.pos) + + tuple(rigid_object_cfg.init_state.rot) + + tuple(rigid_object_cfg.init_state.lin_vel) + + tuple(rigid_object_cfg.init_state.ang_vel) + ) + default_object_state = ( + torch.tensor(default_object_state, dtype=torch.float, device=self.device) + .repeat(self.num_instances, 1) + .unsqueeze(1) + ) + default_object_states.append(default_object_state) + # concatenate the default state for each object + default_object_states = torch.cat(default_object_states, dim=1) + self._data.default_object_state = default_object_states + + def _env_obj_ids_to_view_ids( + self, env_ids: torch.Tensor, object_ids: Sequence[int] | slice | torch.Tensor + ) -> torch.Tensor: + """Converts environment and object indices to indices consistent with data from :attr:`root_physx_view`. + + Args: + env_ids: Environment indices. + object_ids: Object indices. + + Returns: + The view indices. + """ + # the order is env_0/object_0, env_0/object_1, env_0/object_..., env_1/object_0, env_1/object_1, ... + # return a flat tensor of indices + if isinstance(object_ids, slice): + object_ids = self._ALL_OBJ_INDICES + elif isinstance(object_ids, Sequence): + object_ids = torch.tensor(object_ids, device=self.device) + return (object_ids.unsqueeze(1) * self.num_instances + env_ids).flatten() + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._root_physx_view = None + + def _on_prim_deletion(self, prim_path: str) -> None: + """Invalidates and deletes the callbacks when the prim is deleted. + + Args: + prim_path: The path to the prim that is being deleted. + + Note: + This function is called when the prim is deleted. + """ + if prim_path == "/": + self._clear_callbacks() + return + for prim_path_expr in self._prim_paths: + result = re.match( + pattern="^" + "/".join(prim_path_expr.split("/")[: prim_path.count("/") + 1]) + "$", string=prim_path + ) + if result: + self._clear_callbacks() + return diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py new file mode 100644 index 00000000000..f1d9e57d304 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_cfg.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.assets.rigid_object import RigidObjectCfg +from isaaclab.utils import configclass + +from .rigid_object_collection import RigidObjectCollection + + +@configclass +class RigidObjectCollectionCfg: + """Configuration parameters for a rigid object collection.""" + + class_type: type = RigidObjectCollection + """The associated asset class. + + The class should inherit from :class:`isaaclab.assets.asset_base.AssetBase`. + """ + + rigid_objects: dict[str, RigidObjectCfg] = MISSING + """Dictionary of rigid object configurations to spawn. + + The keys are the names for the objects, which are used as unique identifiers throughout the code. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py new file mode 100644 index 00000000000..8f3a54d4a1b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py @@ -0,0 +1,514 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +import weakref + +import omni.physics.tensors.impl.api as physx + +import isaaclab.utils.math as math_utils +from isaaclab.utils.buffers import TimestampedBuffer + + +class RigidObjectCollectionData: + """Data container for a rigid object collection. + + This class contains the data for a rigid object collection in the simulation. The data includes the state of + all the bodies in the collection. The data is stored in the simulation world frame unless otherwise specified. + The data is in the order ``(num_instances, num_objects, data_size)``, where data_size is the size of the data. + + For a rigid body, there are two frames of reference that are used: + + - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim + with the rigid body schema. + - Center of mass frame: The frame of reference of the center of mass of the rigid body. + + Depending on the settings of the simulation, the actor frame and the center of mass frame may be the same. + This needs to be taken into account when interpreting the data. + + The data is lazily updated, meaning that the data is only updated when it is accessed. This is useful + when the data is expensive to compute or retrieve. The data is updated when the timestamp of the buffer + is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. + """ + + def __init__(self, root_physx_view: physx.RigidBodyView, num_objects: int, device: str): + """Initializes the data. + + Args: + root_physx_view: The root rigid body view. + num_objects: The number of objects in the collection. + device: The device used for processing. + """ + # Set the parameters + self.device = device + self.num_objects = num_objects + # Set the root rigid body view + # note: this is stored as a weak reference to avoid circular references between the asset class + # and the data container. This is important to avoid memory leaks. + self._root_physx_view: physx.RigidBodyView = weakref.proxy(root_physx_view) + self.num_instances = self._root_physx_view.count // self.num_objects + + # Set initial time stamp + self._sim_timestamp = 0.0 + + # Obtain global physics sim view + physics_sim_view = physx.create_simulation_view("torch") + physics_sim_view.set_subspace_roots("/") + gravity = physics_sim_view.get_gravity() + # Convert to direction vector + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + gravity_dir = math_utils.normalize(gravity_dir.unsqueeze(0)).squeeze(0) + + # Initialize constants + self.GRAVITY_VEC_W = gravity_dir.repeat(self.num_instances, self.num_objects, 1) + self.FORWARD_VEC_B = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat( + self.num_instances, self.num_objects, 1 + ) + + # Initialize the lazy buffers. + # -- link frame w.r.t. world frame + self._object_link_pose_w = TimestampedBuffer() + self._object_link_vel_w = TimestampedBuffer() + # -- com frame w.r.t. link frame + self._object_com_pose_b = TimestampedBuffer() + # -- com frame w.r.t. world frame + self._object_com_pose_w = TimestampedBuffer() + self._object_com_vel_w = TimestampedBuffer() + self._object_com_acc_w = TimestampedBuffer() + # -- combined state(these are cached as they concatenate) + self._object_state_w = TimestampedBuffer() + self._object_link_state_w = TimestampedBuffer() + self._object_com_state_w = TimestampedBuffer() + + def update(self, dt: float): + """Updates the data for the rigid object collection. + + Args: + dt: The time step for the update. This must be a positive value. + """ + # update the simulation timestamp + self._sim_timestamp += dt + + ## + # Names. + ## + + object_names: list[str] = None + """Object names in the order parsed by the simulation view.""" + + ## + # Defaults. + ## + + default_object_state: torch.Tensor = None + """Default object state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. + Shape is (num_instances, num_objects, 13). + + The position and quaternion are of each object's rigid body's actor frame. Meanwhile, the linear and + angular velocities are of the center of mass frame. + """ + + default_mass: torch.Tensor = None + """Default object mass read from the simulation. Shape is (num_instances, num_objects, 1).""" + + default_inertia: torch.Tensor = None + """Default object inertia tensor read from the simulation. Shape is (num_instances, num_objects, 9). + + The inertia is the inertia tensor relative to the center of mass frame. The values are stored in + the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + """ + + ## + # Root state properties. + ## + + @property + def object_link_pose_w(self): + """Object link pose ``[pos, quat]`` in simulation world frame. Shape is (num_instances, num_objects, 7). + + The position and orientation are of the rigid body's actor frame. + """ + if self._object_link_pose_w.timestamp < self._sim_timestamp: + # read data from simulation + pose = self._reshape_view_to_data(self._root_physx_view.get_transforms().clone()) + pose[..., 3:7] = math_utils.convert_quat(pose[..., 3:7], to="wxyz") + # set the buffer data and timestamp + self._object_link_pose_w.data = pose + self._object_link_pose_w.timestamp = self._sim_timestamp + + return self._object_link_pose_w.data + + @property + def object_link_vel_w(self): + """Object link velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_objects, 6). + + The linear and angular velocities are of the rigid body's actor frame. + """ + if self._object_link_vel_w.timestamp < self._sim_timestamp: + # read data from simulation + velocity = self.object_com_vel_w.clone() + # adjust linear velocity to link from center of mass + velocity[..., :3] += torch.linalg.cross( + velocity[..., 3:], math_utils.quat_rotate(self.object_link_quat_w, -self.object_com_pos_b), dim=-1 + ) + # set the buffer data and timestamp + self._object_link_vel_w.data = velocity + self._object_link_vel_w.timestamp = self._sim_timestamp + + return self._object_link_vel_w.data + + @property + def object_com_pose_w(self): + """Object center of mass pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances, num_objects, 7). + + The position and orientation are of the rigid body's center of mass frame. + """ + if self._object_com_pose_w.timestamp < self._sim_timestamp: + # adjust pose to center of mass + pos, quat = math_utils.combine_frame_transforms( + self.object_link_pos_w, self.object_link_quat_w, self.object_com_pos_b, self.object_com_quat_b + ) + # set the buffer data and timestamp + self._object_com_pose_w.data = torch.cat((pos, quat), dim=-1) + self._object_com_pose_w.timestamp = self._sim_timestamp + + return self._object_com_pose_w.data + + @property + def object_com_vel_w(self): + """Object center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_objects, 6). + + The linear and angular velocities are of the rigid body's center of mass frame. + """ + if self._object_com_vel_w.timestamp < self._sim_timestamp: + self._object_com_vel_w.data = self._reshape_view_to_data(self._root_physx_view.get_velocities()) + self._object_com_vel_w.timestamp = self._sim_timestamp + + return self._object_com_vel_w.data + + @property + def object_state_w(self): + """Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_objects, 13). + + The position and orientation are of the rigid body's actor frame. Meanwhile, the linear and angular + velocities are of the rigid body's center of mass frame. + """ + if self._object_state_w.timestamp < self._sim_timestamp: + self._object_state_w.data = torch.cat((self.object_link_pose_w, self.object_com_vel_w), dim=-1) + self._object_state_w.timestamp = self._sim_timestamp + + return self._object_state_w.data + + @property + def object_link_state_w(self): + """Object center of mass state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_objects, 13). + + The position, quaternion, and linear/angular velocity are of the rigid body root frame relative to the + world. + """ + if self._object_link_state_w.timestamp < self._sim_timestamp: + self._object_link_state_w.data = torch.cat((self.object_link_pose_w, self.object_link_vel_w), dim=-1) + self._object_link_state_w.timestamp = self._sim_timestamp + + return self._object_link_state_w.data + + @property + def object_com_state_w(self): + """Object state ``[pos, quat, lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_objects, 13). + + The position, quaternion, and linear/angular velocity are of the rigid body's center of mass frame + relative to the world. Center of mass frame has the orientation along the principle axes of inertia. + """ + if self._object_com_state_w.timestamp < self._sim_timestamp: + self._object_com_state_w.data = torch.cat((self.object_com_pose_w, self.object_com_vel_w), dim=-1) + self._object_com_state_w.timestamp = self._sim_timestamp + + return self._object_com_state_w.data + + @property + def object_com_acc_w(self): + """Acceleration of all objects. Shape is (num_instances, num_objects, 6). + + This quantity is the acceleration of the rigid bodies' center of mass frame. + """ + if self._object_com_acc_w.timestamp < self._sim_timestamp: + self._object_com_acc_w.data = self._reshape_view_to_data(self._root_physx_view.get_accelerations()) + self._object_com_acc_w.timestamp = self._sim_timestamp + return self._object_com_acc_w.data + + @property + def object_com_pose_b(self): + """Object center of mass pose ``[pos, quat]`` in their respective body's link frame. + Shape is (num_instances, num_objects, 7). + + The position and orientation are of the rigid body's center of mass frame. + The orientation is provided in (w, x, y, z) format. + """ + if self._object_com_pose_b.timestamp < self._sim_timestamp: + # obtain the coms + poses = self._root_physx_view.get_coms().to(self.device) + poses[:, 3:7] = math_utils.convert_quat(poses[:, 3:7], to="wxyz") + # read data from simulation + self._object_com_pose_b.data = self._reshape_view_to_data(poses) + self._object_com_pose_b.timestamp = self._sim_timestamp + + return self._object_com_pose_b.data + + ## + # Derived properties. + ## + + @property + def projected_gravity_b(self): + """Projection of the gravity direction on base frame. Shape is (num_instances, num_objects, 3).""" + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.GRAVITY_VEC_W) + + @property + def heading_w(self): + """Yaw heading of the base frame (in radians). Shape is (num_instances, num_objects,). + + Note: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + forward_w = math_utils.quat_apply(self.object_link_quat_w, self.FORWARD_VEC_B) + return torch.atan2(forward_w[..., 1], forward_w[..., 0]) + + @property + def object_link_lin_vel_b(self) -> torch.Tensor: + """Object link linear velocity in base frame. Shape is (num_instances, num_objects, 3). + + This quantity is the linear velocity of the actor frame of the root rigid body frame with + respect to the rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_lin_vel_w) + + @property + def object_link_ang_vel_b(self) -> torch.Tensor: + """Object link angular velocity in base world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the angular velocity of the actor frame of the root rigid body frame with + respect to the rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_ang_vel_w) + + @property + def object_com_lin_vel_b(self) -> torch.Tensor: + """Object center of mass linear velocity in base frame. Shape is (num_instances, num_objects, 3). + + This quantity is the linear velocity of the center of mass frame of the root rigid body frame with + respect to the rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_lin_vel_w) + + @property + def object_com_ang_vel_b(self) -> torch.Tensor: + """Object center of mass angular velocity in base world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the angular velocity of the center of mass frame of the root rigid body frame with + respect to the rigid body's actor frame. + """ + return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_ang_vel_w) + + ## + # Sliced properties. + ## + + @property + def object_link_pos_w(self) -> torch.Tensor: + """Object link position in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the position of the actor frame of the rigid bodies. + """ + return self.object_link_pose_w[..., :3] + + @property + def object_link_quat_w(self) -> torch.Tensor: + """Object link orientation (w, x, y, z) in simulation world frame. Shape is (num_instances, num_objects, 4). + + This quantity is the orientation of the actor frame of the rigid bodies. + """ + return self.object_link_pose_w[..., 3:7] + + @property + def object_link_lin_vel_w(self) -> torch.Tensor: + """Object link linear velocity in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the linear velocity of the rigid bodies' actor frame. + """ + return self.object_link_vel_w[..., :3] + + @property + def object_link_ang_vel_w(self) -> torch.Tensor: + """Object link angular velocity in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the angular velocity of the rigid bodies' actor frame. + """ + return self.object_link_vel_w[..., 3:6] + + @property + def object_com_pos_w(self) -> torch.Tensor: + """Object center of mass position in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the position of the center of mass frame of the rigid bodies. + """ + return self.object_com_pose_w[..., :3] + + @property + def object_com_quat_w(self) -> torch.Tensor: + """Object center of mass orientation (w, x, y, z) in simulation world frame. + Shape is (num_instances, num_objects, 4). + + This quantity is the orientation of the center of mass frame of the rigid bodies. + """ + return self.object_com_pose_w[..., 3:7] + + @property + def object_com_lin_vel_w(self) -> torch.Tensor: + """Object center of mass linear velocity in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the linear velocity of the rigid bodies' center of mass frame. + """ + return self.object_com_vel_w[..., :3] + + @property + def object_com_ang_vel_w(self) -> torch.Tensor: + """Object center of mass angular velocity in simulation world frame. Shape is (num_instances, num_objects, 3). + + This quantity is the angular velocity of the rigid bodies' center of mass frame. + """ + return self.object_com_vel_w[..., 3:6] + + @property + def object_com_lin_acc_w(self) -> torch.Tensor: + """Object center of mass linear acceleration in simulation world frame. + Shape is (num_instances, num_objects, 3). + + This quantity is the linear acceleration of the rigid bodies' center of mass frame. + """ + return self.object_com_acc_w[..., :3] + + @property + def object_com_ang_acc_w(self) -> torch.Tensor: + """Object center of mass angular acceleration in simulation world frame. + Shape is (num_instances, num_objects, 3). + + This quantity is the angular acceleration of the rigid bodies' center of mass frame. + """ + return self.object_com_acc_w[..., 3:6] + + @property + def object_com_pos_b(self) -> torch.Tensor: + """Center of mass of all of the bodies in their respective body's link frame. + Shape is (num_instances, num_objects, 3). + + This quantity is the center of mass location relative to its body link frame. + """ + return self.object_com_pose_b[..., :3] + + @property + def object_com_quat_b(self) -> torch.Tensor: + """Orientation (w,x,y,z) of the principle axis of inertia of all of the bodies in simulation world frame. + Shape is (num_instances, num_objects, 4). + + This quantity is the orientation of the principles axes of inertia relative to its body link frame. + The orientation is provided in (w, x, y, z) format. + """ + return self.object_com_pose_b[..., 3:7] + + ## + # Properties for backwards compatibility. + ## + + @property + def object_pose_w(self) -> torch.Tensor: + """Same as :attr:`object_link_pose_w`.""" + return self.object_link_pose_w + + @property + def object_pos_w(self) -> torch.Tensor: + """Same as :attr:`object_link_pos_w`.""" + return self.object_link_pos_w + + @property + def object_quat_w(self) -> torch.Tensor: + """Same as :attr:`object_link_quat_w`.""" + return self.object_link_quat_w + + @property + def object_vel_w(self) -> torch.Tensor: + """Same as :attr:`object_com_vel_w`.""" + return self.object_com_vel_w + + @property + def object_lin_vel_w(self) -> torch.Tensor: + """Same as :attr:`object_com_lin_vel_w`.""" + return self.object_com_lin_vel_w + + @property + def object_ang_vel_w(self) -> torch.Tensor: + """Same as :attr:`object_com_ang_vel_w`.""" + return self.object_com_ang_vel_w + + @property + def object_lin_vel_b(self) -> torch.Tensor: + """Same as :attr:`object_com_lin_vel_b`.""" + return self.object_com_lin_vel_b + + @property + def object_ang_vel_b(self) -> torch.Tensor: + """Same as :attr:`object_com_ang_vel_b`.""" + return self.object_com_ang_vel_b + + @property + def object_acc_w(self) -> torch.Tensor: + """Same as :attr:`object_com_acc_w`.""" + return self.object_com_acc_w + + @property + def object_lin_acc_w(self) -> torch.Tensor: + """Same as :attr:`object_com_lin_acc_w`.""" + return self.object_com_lin_acc_w + + @property + def object_ang_acc_w(self) -> torch.Tensor: + """Same as :attr:`object_com_ang_acc_w`.""" + return self.object_com_ang_acc_w + + @property + def com_pos_b(self) -> torch.Tensor: + """Same as :attr:`object_com_pos_b`.""" + return self.object_com_pos_b + + @property + def com_quat_b(self) -> torch.Tensor: + """Same as :attr:`object_com_quat_b`.""" + return self.object_com_quat_b + + ## + # Helpers. + ## + + def _reshape_view_to_data(self, data: torch.Tensor) -> torch.Tensor: + """Reshapes and arranges the data from the physics view to (num_instances, num_objects, data_size). + + Args: + data: The data from the physics view. Shape is (num_instances * num_objects, data_size). + + Returns: + The reshaped data. Shape is (num_objects, num_instances, data_size). + """ + return torch.einsum("ijk -> jik", data.reshape(self.num_objects, self.num_instances, -1)) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/__init__.py new file mode 100644 index 00000000000..90126dd5c06 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/__init__.py @@ -0,0 +1,22 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for different controllers and motion-generators. + +Controllers or motion generators are responsible for closed-loop tracking of a given command. The +controller can be a simple PID controller or a more complex controller such as impedance control +or inverse kinematics control. The controller is responsible for generating the desired joint-level +commands to be sent to the robot. +""" + +from .differential_ik import DifferentialIKController +from .differential_ik_cfg import DifferentialIKControllerCfg +from .operational_space import OperationalSpaceController +from .operational_space_cfg import OperationalSpaceControllerCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/data/lula_franka_gen.urdf b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/data/lula_franka_gen.urdf new file mode 100644 index 00000000000..2d00a71e12d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/data/lula_franka_gen.urdf @@ -0,0 +1,415 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/rmp_flow.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/rmp_flow.py new file mode 100644 index 00000000000..4c9d93d4fd5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/config/rmp_flow.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os + +from isaacsim.core.utils.extensions import get_extension_path_from_name + +from isaaclab.controllers.rmp_flow import RmpFlowControllerCfg + +# Note: RMP-Flow config files for supported robots are stored in the motion_generation extension +_RMP_CONFIG_DIR = os.path.join( + get_extension_path_from_name("isaacsim.robot_motion.motion_generation"), "motion_policy_configs" +) + +# Path to current directory +_CUR_DIR = os.path.dirname(os.path.realpath(__file__)) + +FRANKA_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "franka_rmpflow_common.yaml"), + urdf_file=os.path.join(_CUR_DIR, "data", "lula_franka_gen.urdf"), + collision_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "robot_descriptor.yaml"), + frame_name="panda_end_effector", + evaluations_per_frame=5, +) +"""Configuration of RMPFlow for Franka arm (default from `isaacsim.robot_motion.motion_generation`).""" + + +UR10_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "rmpflow", "ur10_rmpflow_config.yaml"), + urdf_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "ur10_robot.urdf"), + collision_file=os.path.join(_RMP_CONFIG_DIR, "ur10", "rmpflow", "ur10_robot_description.yaml"), + frame_name="ee_link", + evaluations_per_frame=5, +) +"""Configuration of RMPFlow for UR10 arm (default from `isaacsim.robot_motion.motion_generation`).""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik.py new file mode 100644 index 00000000000..ad4cf95f767 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik.py @@ -0,0 +1,245 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.utils.math import apply_delta_pose, compute_pose_error + +if TYPE_CHECKING: + from .differential_ik_cfg import DifferentialIKControllerCfg + + +class DifferentialIKController: + r"""Differential inverse kinematics (IK) controller. + + This controller is based on the concept of differential inverse kinematics [1, 2] which is a method for computing + the change in joint positions that yields the desired change in pose. + + .. math:: + + \Delta \mathbf{q} &= \mathbf{J}^{\dagger} \Delta \mathbf{x} \\ + \mathbf{q}_{\text{desired}} &= \mathbf{q}_{\text{current}} + \Delta \mathbf{q} + + where :math:`\mathbf{J}^{\dagger}` is the pseudo-inverse of the Jacobian matrix :math:`\mathbf{J}`, + :math:`\Delta \mathbf{x}` is the desired change in pose, and :math:`\mathbf{q}_{\text{current}}` + is the current joint positions. + + To deal with singularity in Jacobian, the following methods are supported for computing inverse of the Jacobian: + + - "pinv": Moore-Penrose pseudo-inverse + - "svd": Adaptive singular-value decomposition (SVD) + - "trans": Transpose of matrix + - "dls": Damped version of Moore-Penrose pseudo-inverse (also called Levenberg-Marquardt) + + + .. caution:: + The controller does not assume anything about the frames of the current and desired end-effector pose, + or the joint-space velocities. It is up to the user to ensure that these quantities are given + in the correct format. + + Reference: + + 1. `Robot Dynamics Lecture Notes `_ + by Marco Hutter (ETH Zurich) + 2. `Introduction to Inverse Kinematics `_ + by Samuel R. Buss (University of California, San Diego) + + """ + + def __init__(self, cfg: DifferentialIKControllerCfg, num_envs: int, device: str): + """Initialize the controller. + + Args: + cfg: The configuration for the controller. + num_envs: The number of environments. + device: The device to use for computations. + """ + # store inputs + self.cfg = cfg + self.num_envs = num_envs + self._device = device + # create buffers + self.ee_pos_des = torch.zeros(self.num_envs, 3, device=self._device) + self.ee_quat_des = torch.zeros(self.num_envs, 4, device=self._device) + # -- input command + self._command = torch.zeros(self.num_envs, self.action_dim, device=self._device) + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the controller's input command.""" + if self.cfg.command_type == "position": + return 3 # (x, y, z) + elif self.cfg.command_type == "pose" and self.cfg.use_relative_mode: + return 6 # (dx, dy, dz, droll, dpitch, dyaw) + else: + return 7 # (x, y, z, qw, qx, qy, qz) + + """ + Operations. + """ + + def reset(self, env_ids: torch.Tensor = None): + """Reset the internals. + + Args: + env_ids: The environment indices to reset. If None, then all environments are reset. + """ + pass + + def set_command( + self, command: torch.Tensor, ee_pos: torch.Tensor | None = None, ee_quat: torch.Tensor | None = None + ): + """Set target end-effector pose command. + + Based on the configured command type and relative mode, the method computes the desired end-effector pose. + It is up to the user to ensure that the command is given in the correct frame. The method only + applies the relative mode if the command type is ``position_rel`` or ``pose_rel``. + + Args: + command: The input command in shape (N, 3) or (N, 6) or (N, 7). + ee_pos: The current end-effector position in shape (N, 3). + This is only needed if the command type is ``position_rel`` or ``pose_rel``. + ee_quat: The current end-effector orientation (w, x, y, z) in shape (N, 4). + This is only needed if the command type is ``position_*`` or ``pose_rel``. + + Raises: + ValueError: If the command type is ``position_*`` and :attr:`ee_quat` is None. + ValueError: If the command type is ``position_rel`` and :attr:`ee_pos` is None. + ValueError: If the command type is ``pose_rel`` and either :attr:`ee_pos` or :attr:`ee_quat` is None. + """ + # store command + self._command[:] = command + # compute the desired end-effector pose + if self.cfg.command_type == "position": + # we need end-effector orientation even though we are in position mode + # this is only needed for display purposes + if ee_quat is None: + raise ValueError("End-effector orientation can not be None for `position_*` command type!") + # compute targets + if self.cfg.use_relative_mode: + if ee_pos is None: + raise ValueError("End-effector position can not be None for `position_rel` command type!") + self.ee_pos_des[:] = ee_pos + self._command + self.ee_quat_des[:] = ee_quat + else: + self.ee_pos_des[:] = self._command + self.ee_quat_des[:] = ee_quat + else: + # compute targets + if self.cfg.use_relative_mode: + if ee_pos is None or ee_quat is None: + raise ValueError( + "Neither end-effector position nor orientation can be None for `pose_rel` command type!" + ) + self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command) + else: + self.ee_pos_des = self._command[:, 0:3] + self.ee_quat_des = self._command[:, 3:7] + + def compute( + self, ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor, joint_pos: torch.Tensor + ) -> torch.Tensor: + """Computes the target joint positions that will yield the desired end effector pose. + + Args: + ee_pos: The current end-effector position in shape (N, 3). + ee_quat: The current end-effector orientation in shape (N, 4). + jacobian: The geometric jacobian matrix in shape (N, 6, num_joints). + joint_pos: The current joint positions in shape (N, num_joints). + + Returns: + The target joint positions commands in shape (N, num_joints). + """ + # compute the delta in joint-space + if "position" in self.cfg.command_type: + position_error = self.ee_pos_des - ee_pos + jacobian_pos = jacobian[:, 0:3] + delta_joint_pos = self._compute_delta_joint_pos(delta_pose=position_error, jacobian=jacobian_pos) + else: + position_error, axis_angle_error = compute_pose_error( + ee_pos, ee_quat, self.ee_pos_des, self.ee_quat_des, rot_error_type="axis_angle" + ) + pose_error = torch.cat((position_error, axis_angle_error), dim=1) + delta_joint_pos = self._compute_delta_joint_pos(delta_pose=pose_error, jacobian=jacobian) + # return the desired joint positions + return joint_pos + delta_joint_pos + + """ + Helper functions. + """ + + def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Tensor) -> torch.Tensor: + """Computes the change in joint position that yields the desired change in pose. + + The method uses the Jacobian mapping from joint-space velocities to end-effector velocities + to compute the delta-change in the joint-space that moves the robot closer to a desired + end-effector position. + + Args: + delta_pose: The desired delta pose in shape (N, 3) or (N, 6). + jacobian: The geometric jacobian matrix in shape (N, 3, num_joints) or (N, 6, num_joints). + + Returns: + The desired delta in joint space. Shape is (N, num-jointsß). + """ + if self.cfg.ik_params is None: + raise RuntimeError(f"Inverse-kinematics parameters for method '{self.cfg.ik_method}' is not defined!") + # compute the delta in joint-space + if self.cfg.ik_method == "pinv": # Jacobian pseudo-inverse + # parameters + k_val = self.cfg.ik_params["k_val"] + # computation + jacobian_pinv = torch.linalg.pinv(jacobian) + delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "svd": # adaptive SVD + # parameters + k_val = self.cfg.ik_params["k_val"] + min_singular_value = self.cfg.ik_params["min_singular_value"] + # computation + # U: 6xd, S: dxd, V: d x num-joint + U, S, Vh = torch.linalg.svd(jacobian) + S_inv = 1.0 / S + S_inv = torch.where(S > min_singular_value, S_inv, torch.zeros_like(S_inv)) + jacobian_pinv = ( + torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] + @ torch.diag_embed(S_inv) + @ torch.transpose(U, dim0=1, dim1=2) + ) + delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "trans": # Jacobian transpose + # parameters + k_val = self.cfg.ik_params["k_val"] + # computation + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + delta_joint_pos = k_val * jacobian_T @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "dls": # damped least squares + # parameters + lambda_val = self.cfg.ik_params["lambda_val"] + # computation + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=self._device) + delta_joint_pos = ( + jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1) + ) + delta_joint_pos = delta_joint_pos.squeeze(-1) + else: + raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}") + + return delta_joint_pos diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik_cfg.py new file mode 100644 index 00000000000..eed31daf778 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/differential_ik_cfg.py @@ -0,0 +1,75 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .differential_ik import DifferentialIKController + + +@configclass +class DifferentialIKControllerCfg: + """Configuration for differential inverse kinematics controller.""" + + class_type: type = DifferentialIKController + """The associated controller class.""" + + command_type: Literal["position", "pose"] = MISSING + """Type of task-space command to control the articulation's body. + + If "position", then the controller only controls the position of the articulation's body. + Otherwise, the controller controls the pose of the articulation's body. + """ + + use_relative_mode: bool = False + """Whether to use relative mode for the controller. Defaults to False. + + If True, then the controller treats the input command as a delta change in the position/pose. + Otherwise, the controller treats the input command as the absolute position/pose. + """ + + ik_method: Literal["pinv", "svd", "trans", "dls"] = MISSING + """Method for computing inverse of Jacobian.""" + + ik_params: dict[str, float] | None = None + """Parameters for the inverse-kinematics method. Defaults to None, in which case the default + parameters for the method are used. + + - Moore-Penrose pseudo-inverse ("pinv"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - Adaptive Singular Value Decomposition ("svd"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - "min_singular_value": Single values less than this are suppressed to zero (default: 1e-5). + - Jacobian transpose ("trans"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - Damped Moore-Penrose pseudo-inverse ("dls"): + - "lambda_val": Damping coefficient (default: 0.01). + """ + + def __post_init__(self): + # check valid input + if self.command_type not in ["position", "pose"]: + raise ValueError(f"Unsupported inverse-kinematics command: {self.command_type}.") + if self.ik_method not in ["pinv", "svd", "trans", "dls"]: + raise ValueError(f"Unsupported inverse-kinematics method: {self.ik_method}.") + # default parameters for different inverse kinematics approaches. + default_ik_params = { + "pinv": {"k_val": 1.0}, + "svd": {"k_val": 1.0, "min_singular_value": 1e-5}, + "trans": {"k_val": 1.0}, + "dls": {"lambda_val": 0.01}, + } + # update parameters for IK-method if not provided + ik_params = default_ik_params[self.ik_method].copy() + if self.ik_params is not None: + ik_params.update(self.ik_params) + self.ik_params = ik_params diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/joint_impedance.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/joint_impedance.py new file mode 100644 index 00000000000..ac3cc072b54 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/joint_impedance.py @@ -0,0 +1,234 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from collections.abc import Sequence +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class JointImpedanceControllerCfg: + """Configuration for joint impedance regulation controller.""" + + command_type: str = "p_abs" + """Type of command: p_abs (absolute) or p_rel (relative).""" + + dof_pos_offset: Sequence[float] | None = None + """Offset to DOF position command given to controller. (default: None). + + If None then position offsets are set to zero. + """ + + impedance_mode: str = MISSING + """Type of gains: "fixed", "variable", "variable_kp".""" + + inertial_compensation: bool = False + """Whether to perform inertial compensation (inverse dynamics).""" + + gravity_compensation: bool = False + """Whether to perform gravity compensation.""" + + stiffness: float | Sequence[float] = MISSING + """The positional gain for determining desired torques based on joint position error.""" + + damping_ratio: float | Sequence[float] | None = None + """The damping ratio is used in-conjunction with positional gain to compute desired torques + based on joint velocity error. + + The following math operation is performed for computing velocity gains: + :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. + """ + + stiffness_limits: tuple[float, float] = (0, 300) + """Minimum and maximum values for positional gains. + + Note: Used only when :obj:`impedance_mode` is "variable" or "variable_kp". + """ + + damping_ratio_limits: tuple[float, float] = (0, 100) + """Minimum and maximum values for damping ratios used to compute velocity gains. + + Note: Used only when :obj:`impedance_mode` is "variable". + """ + + +class JointImpedanceController: + """Joint impedance regulation control. + + Reference: + [1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + """ + + def __init__(self, cfg: JointImpedanceControllerCfg, num_robots: int, dof_pos_limits: torch.Tensor, device: str): + """Initialize joint impedance controller. + + Args: + cfg: The configuration for the controller. + num_robots: The number of robots to control. + dof_pos_limits: The joint position limits for each robot. This is a tensor of shape + (num_robots, num_dof, 2) where the last dimension contains the lower and upper limits. + device: The device to use for computations. + + Raises: + ValueError: When the shape of :obj:`dof_pos_limits` is not (num_robots, num_dof, 2). + """ + # check valid inputs + if len(dof_pos_limits.shape) != 3: + raise ValueError(f"Joint position limits has shape '{dof_pos_limits.shape}'. Expected length of shape = 3.") + # store inputs + self.cfg = cfg + self.num_robots = num_robots + self.num_dof = dof_pos_limits.shape[1] # (num_robots, num_dof, 2) + self._device = device + + # create buffers + # -- commands + self._dof_pos_target = torch.zeros(self.num_robots, self.num_dof, device=self._device) + # -- offsets + self._dof_pos_offset = torch.zeros(self.num_robots, self.num_dof, device=self._device) + # -- limits + self._dof_pos_limits = dof_pos_limits + # -- positional gains + self._p_gains = torch.zeros(self.num_robots, self.num_dof, device=self._device) + self._p_gains[:] = torch.tensor(self.cfg.stiffness, device=self._device) + # -- velocity gains + self._d_gains = torch.zeros(self.num_robots, self.num_dof, device=self._device) + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * torch.tensor(self.cfg.damping_ratio, device=self._device) + # -- position offsets + if self.cfg.dof_pos_offset is not None: + self._dof_pos_offset[:] = torch.tensor(self.cfg.dof_pos_offset, device=self._device) + # -- position gain limits + self._p_gains_limits = torch.zeros_like(self._dof_pos_limits) + self._p_gains_limits[..., 0] = self.cfg.stiffness_limits[0] + self._p_gains_limits[..., 1] = self.cfg.stiffness_limits[1] + # -- damping ratio limits + self._damping_ratio_limits = torch.zeros_like(self._dof_pos_limits) + self._damping_ratio_limits[..., 0] = self.cfg.damping_ratio_limits[0] + self._damping_ratio_limits[..., 1] = self.cfg.damping_ratio_limits[1] + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + # impedance mode + if self.cfg.impedance_mode == "fixed": + # joint positions + return self.num_dof + elif self.cfg.impedance_mode == "variable_kp": + # joint positions + stiffness + return self.num_dof * 2 + elif self.cfg.impedance_mode == "variable": + # joint positions + stiffness + damping + return self.num_dof * 3 + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + """ + Operations. + """ + + def initialize(self): + """Initialize the internals.""" + pass + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + pass + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command. + + Args: + command: The command to set. This is a tensor of shape (num_robots, num_actions) where + :obj:`num_actions` is the dimension of the action space of the controller. + """ + # check input size + if command.shape != (self.num_robots, self.num_actions): + raise ValueError( + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_robots, self.num_actions)}'." + ) + # impedance mode + if self.cfg.impedance_mode == "fixed": + # joint positions + self._dof_pos_target[:] = command + elif self.cfg.impedance_mode == "variable_kp": + # split input command + dof_pos_command, stiffness = torch.tensor_split(command, 2, dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + # joint positions + stiffness + self._dof_pos_target[:] = dof_pos_command + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) # critically damped + elif self.cfg.impedance_mode == "variable": + # split input command + dof_pos_command, stiffness, damping_ratio = torch.tensor_split(command, 3, dim=-1) + # format command + stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) + damping_ratio = damping_ratio.clip_(min=self._damping_ratio_limits[0], max=self._damping_ratio_limits[1]) + # joint positions + stiffness + damping + self._dof_pos_target[:] = dof_pos_command + self._p_gains[:] = stiffness + self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * damping_ratio + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + def compute( + self, + dof_pos: torch.Tensor, + dof_vel: torch.Tensor, + mass_matrix: torch.Tensor | None = None, + gravity: torch.Tensor | None = None, + ) -> torch.Tensor: + """Performs inference with the controller. + + Args: + dof_pos: The current joint positions. + dof_vel: The current joint velocities. + mass_matrix: The joint-space inertial matrix. Defaults to None. + gravity: The joint-space gravity vector. Defaults to None. + + Raises: + ValueError: When the command type is invalid. + + Returns: + The target joint torques commands. + """ + # resolve the command type + if self.cfg.command_type == "p_abs": + desired_dof_pos = self._dof_pos_target + self._dof_pos_offset + elif self.cfg.command_type == "p_rel": + desired_dof_pos = self._dof_pos_target + dof_pos + else: + raise ValueError(f"Invalid dof position command mode: {self.cfg.command_type}.") + # compute errors + desired_dof_pos = desired_dof_pos.clip_(min=self._dof_pos_limits[..., 0], max=self._dof_pos_limits[..., 1]) + dof_pos_error = desired_dof_pos - dof_pos + dof_vel_error = -dof_vel + # compute acceleration + des_dof_acc = self._p_gains * dof_pos_error + self._d_gains * dof_vel_error + # compute torques + # -- inertial compensation + if self.cfg.inertial_compensation: + # inverse dynamics control + desired_torques = mass_matrix @ des_dof_acc + else: + # decoupled spring-mass control + desired_torques = des_dof_acc + # -- gravity compensation (bias correction) + if self.cfg.gravity_compensation: + desired_torques += gravity + + return desired_torques diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space.py new file mode 100644 index 00000000000..2726c0afbc6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space.py @@ -0,0 +1,553 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.utils.math import ( + apply_delta_pose, + combine_frame_transforms, + compute_pose_error, + matrix_from_quat, + subtract_frame_transforms, +) + +if TYPE_CHECKING: + from .operational_space_cfg import OperationalSpaceControllerCfg + + +class OperationalSpaceController: + """Operational-space controller. + + Reference: + + 1. `A unified approach for motion and force control of robot manipulators: The operational space formulation `_ + by Oussama Khatib (Stanford University) + 2. `Robot Dynamics Lecture Notes `_ + by Marco Hutter (ETH Zurich) + """ + + def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: str): + """Initialize operational-space controller. + + Args: + cfg: The configuration for operational-space controller. + num_envs: The number of environments. + device: The device to use for computations. + + Raises: + ValueError: When invalid control command is provided. + """ + # store inputs + self.cfg = cfg + self.num_envs = num_envs + self._device = device + + # resolve tasks-pace target dimensions + self.target_list = list() + for command_type in self.cfg.target_types: + if command_type == "pose_rel": + self.target_list.append(6) + elif command_type == "pose_abs": + self.target_list.append(7) + elif command_type == "wrench_abs": + self.target_list.append(6) + else: + raise ValueError(f"Invalid control command: {command_type}.") + self.target_dim = sum(self.target_list) + + # create buffers + # -- selection matrices, which might be defined in the task reference frame different from the root frame + self._selection_matrix_motion_task = torch.diag_embed( + torch.tensor(self.cfg.motion_control_axes_task, dtype=torch.float, device=self._device) + .unsqueeze(0) + .repeat(self.num_envs, 1) + ) + self._selection_matrix_force_task = torch.diag_embed( + torch.tensor(self.cfg.contact_wrench_control_axes_task, dtype=torch.float, device=self._device) + .unsqueeze(0) + .repeat(self.num_envs, 1) + ) + # -- selection matrices in root frame + self._selection_matrix_motion_b = torch.zeros_like(self._selection_matrix_motion_task) + self._selection_matrix_force_b = torch.zeros_like(self._selection_matrix_force_task) + # -- commands + self._task_space_target_task = torch.zeros(self.num_envs, self.target_dim, device=self._device) + # -- Placeholders for motion/force control + self.desired_ee_pose_task = None + self.desired_ee_pose_b = None + self.desired_ee_wrench_task = None + self.desired_ee_wrench_b = None + # -- buffer for operational space mass matrix + self._os_mass_matrix_b = torch.zeros(self.num_envs, 6, 6, device=self._device) + # -- Placeholder for the inverse of joint space mass matrix + self._mass_matrix_inv = None + # -- motion control gains + self._motion_p_gains_task = torch.diag_embed( + torch.ones(self.num_envs, 6, device=self._device) + * torch.tensor(self.cfg.motion_stiffness_task, dtype=torch.float, device=self._device) + ) + # -- -- zero out the axes that are not motion controlled, as keeping them non-zero will cause other axes + # -- -- to act due to coupling + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task = torch.diag_embed( + 2 + * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() + * torch.as_tensor(self.cfg.motion_damping_ratio_task, dtype=torch.float, device=self._device).reshape(1, -1) + ) + # -- -- motion control gains in root frame + self._motion_p_gains_b = torch.zeros_like(self._motion_p_gains_task) + self._motion_d_gains_b = torch.zeros_like(self._motion_d_gains_task) + # -- force control gains + if self.cfg.contact_wrench_stiffness_task is not None: + self._contact_wrench_p_gains_task = torch.diag_embed( + torch.ones(self.num_envs, 6, device=self._device) + * torch.tensor(self.cfg.contact_wrench_stiffness_task, dtype=torch.float, device=self._device) + ) + self._contact_wrench_p_gains_task[:] = ( + self._selection_matrix_force_task @ self._contact_wrench_p_gains_task[:] + ) + # -- -- force control gains in root frame + self._contact_wrench_p_gains_b = torch.zeros_like(self._contact_wrench_p_gains_task) + else: + self._contact_wrench_p_gains_task = None + self._contact_wrench_p_gains_b = None + # -- position gain limits + self._motion_p_gains_limits = torch.zeros(self.num_envs, 6, 2, device=self._device) + self._motion_p_gains_limits[..., 0], self._motion_p_gains_limits[..., 1] = ( + self.cfg.motion_stiffness_limits_task[0], + self.cfg.motion_stiffness_limits_task[1], + ) + # -- damping ratio limits + self._motion_damping_ratio_limits = torch.zeros_like(self._motion_p_gains_limits) + self._motion_damping_ratio_limits[..., 0], self._motion_damping_ratio_limits[..., 1] = ( + self.cfg.motion_damping_ratio_limits_task[0], + self.cfg.motion_damping_ratio_limits_task[1], + ) + # -- end-effector contact wrench + self._ee_contact_wrench_b = torch.zeros(self.num_envs, 6, device=self._device) + + # -- buffers for null-space control gains + self._nullspace_p_gain = torch.tensor(self.cfg.nullspace_stiffness, dtype=torch.float, device=self._device) + self._nullspace_d_gain = ( + 2 + * torch.sqrt(self._nullspace_p_gain) + * torch.tensor(self.cfg.nullspace_damping_ratio, dtype=torch.float, device=self._device) + ) + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the action space of controller.""" + # impedance mode + if self.cfg.impedance_mode == "fixed": + # task-space targets + return self.target_dim + elif self.cfg.impedance_mode == "variable_kp": + # task-space targets + stiffness + return self.target_dim + 6 + elif self.cfg.impedance_mode == "variable": + # task-space targets + stiffness + damping + return self.target_dim + 6 + 6 + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + """ + Operations. + """ + + def reset(self): + """Reset the internals.""" + self.desired_ee_pose_b = None + self.desired_ee_pose_task = None + self.desired_ee_wrench_b = None + self.desired_ee_wrench_task = None + + def set_command( + self, + command: torch.Tensor, + current_ee_pose_b: torch.Tensor | None = None, + current_task_frame_pose_b: torch.Tensor | None = None, + ): + """Set the task-space targets and impedance parameters. + + Args: + command (torch.Tensor): A concatenated tensor of shape (``num_envs``, ``action_dim``) containing task-space + targets (i.e., pose/wrench) and impedance parameters. + current_ee_pose_b (torch.Tensor, optional): Current end-effector pose, in root frame, of shape + (``num_envs``, 7), containing position and quaternion ``(w, x, y, z)``. Required for relative + commands. Defaults to None. + current_task_frame_pose_b: Current pose of the task frame, in root frame, in which the targets and the + (motion/wrench) control axes are defined. It is a tensor of shape (``num_envs``, 7), + containing position and the quaternion ``(w, x, y, z)``. Defaults to None. + + Format: + Task-space targets, ordered according to 'command_types': + + Absolute pose: shape (``num_envs``, 7), containing position and quaternion ``(w, x, y, z)``. + Relative pose: shape (``num_envs``, 6), containing delta position and rotation in axis-angle form. + Absolute wrench: shape (``num_envs``, 6), containing force and torque. + + Impedance parameters: stiffness for ``variable_kp``, or stiffness, followed by damping ratio for + ``variable``: + + Stiffness: shape (``num_envs``, 6) + Damping ratio: shape (``num_envs``, 6) + + Raises: + ValueError: When the command dimensions are invalid. + ValueError: When an invalid impedance mode is provided. + ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command. + ValueError: When an invalid control command is provided. + """ + # Check the input dimensions + if command.shape != (self.num_envs, self.action_dim): + raise ValueError( + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_envs, self.action_dim)}'." + ) + + # Resolve the impedance parameters + if self.cfg.impedance_mode == "fixed": + # task space targets (i.e., pose/wrench) + self._task_space_target_task[:] = command + elif self.cfg.impedance_mode == "variable_kp": + # split input command + task_space_command, stiffness = torch.split(command, [self.target_dim, 6], dim=-1) + # format command + stiffness = stiffness.clip_( + min=self._motion_p_gains_limits[..., 0], max=self._motion_p_gains_limits[..., 1] + ) + # task space targets + stiffness + self._task_space_target_task[:] = task_space_command.squeeze(dim=-1) + self._motion_p_gains_task[:] = torch.diag_embed(stiffness) + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task = torch.diag_embed( + 2 + * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() + * torch.as_tensor(self.cfg.motion_damping_ratio_task, dtype=torch.float, device=self._device).reshape( + 1, -1 + ) + ) + elif self.cfg.impedance_mode == "variable": + # split input command + task_space_command, stiffness, damping_ratio = torch.split(command, [self.target_dim, 6, 6], dim=-1) + # format command + stiffness = stiffness.clip_( + min=self._motion_p_gains_limits[..., 0], max=self._motion_p_gains_limits[..., 1] + ) + damping_ratio = damping_ratio.clip_( + min=self._motion_damping_ratio_limits[..., 0], max=self._motion_damping_ratio_limits[..., 1] + ) + # task space targets + stiffness + damping + self._task_space_target_task[:] = task_space_command + self._motion_p_gains_task[:] = torch.diag_embed(stiffness) + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task[:] = torch.diag_embed( + 2 * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() * damping_ratio + ) + else: + raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + + if current_task_frame_pose_b is None: + current_task_frame_pose_b = torch.tensor( + [[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]] * self.num_envs, device=self._device + ) + + # Resolve the target commands + target_groups = torch.split(self._task_space_target_task, self.target_list, dim=1) + for command_type, target in zip(self.cfg.target_types, target_groups): + if command_type == "pose_rel": + # check input is provided + if current_ee_pose_b is None: + raise ValueError("Current pose is required for 'pose_rel' command.") + # Transform the current pose from base/root frame to task frame + current_ee_pos_task, current_ee_rot_task = subtract_frame_transforms( + current_task_frame_pose_b[:, :3], + current_task_frame_pose_b[:, 3:], + current_ee_pose_b[:, :3], + current_ee_pose_b[:, 3:], + ) + # compute targets in task frame + desired_ee_pos_task, desired_ee_rot_task = apply_delta_pose( + current_ee_pos_task, current_ee_rot_task, target + ) + self.desired_ee_pose_task = torch.cat([desired_ee_pos_task, desired_ee_rot_task], dim=-1) + elif command_type == "pose_abs": + # compute targets + self.desired_ee_pose_task = target.clone() + elif command_type == "wrench_abs": + # compute targets + self.desired_ee_wrench_task = target.clone() + else: + raise ValueError(f"Invalid control command: {command_type}.") + + # Rotation of task frame wrt root frame, converts a coordinate from task frame to root frame. + R_task_b = matrix_from_quat(current_task_frame_pose_b[:, 3:]) + # Rotation of root frame wrt task frame, converts a coordinate from root frame to task frame. + R_b_task = R_task_b.mT + + # Transform motion control stiffness gains from task frame to root frame + self._motion_p_gains_b[:, 0:3, 0:3] = R_task_b @ self._motion_p_gains_task[:, 0:3, 0:3] @ R_b_task + self._motion_p_gains_b[:, 3:6, 3:6] = R_task_b @ self._motion_p_gains_task[:, 3:6, 3:6] @ R_b_task + + # Transform motion control damping gains from task frame to root frame + self._motion_d_gains_b[:, 0:3, 0:3] = R_task_b @ self._motion_d_gains_task[:, 0:3, 0:3] @ R_b_task + self._motion_d_gains_b[:, 3:6, 3:6] = R_task_b @ self._motion_d_gains_task[:, 3:6, 3:6] @ R_b_task + + # Transform contact wrench gains from task frame to root frame (if applicable) + if self._contact_wrench_p_gains_task is not None and self._contact_wrench_p_gains_b is not None: + self._contact_wrench_p_gains_b[:, 0:3, 0:3] = ( + R_task_b @ self._contact_wrench_p_gains_task[:, 0:3, 0:3] @ R_b_task + ) + self._contact_wrench_p_gains_b[:, 3:6, 3:6] = ( + R_task_b @ self._contact_wrench_p_gains_task[:, 3:6, 3:6] @ R_b_task + ) + + # Transform selection matrices from target frame to base frame + self._selection_matrix_motion_b[:, 0:3, 0:3] = ( + R_task_b @ self._selection_matrix_motion_task[:, 0:3, 0:3] @ R_b_task + ) + self._selection_matrix_motion_b[:, 3:6, 3:6] = ( + R_task_b @ self._selection_matrix_motion_task[:, 3:6, 3:6] @ R_b_task + ) + self._selection_matrix_force_b[:, 0:3, 0:3] = ( + R_task_b @ self._selection_matrix_force_task[:, 0:3, 0:3] @ R_b_task + ) + self._selection_matrix_force_b[:, 3:6, 3:6] = ( + R_task_b @ self._selection_matrix_force_task[:, 3:6, 3:6] @ R_b_task + ) + + # Transform desired pose from task frame to root frame + if self.desired_ee_pose_task is not None: + self.desired_ee_pose_b = torch.zeros_like(self.desired_ee_pose_task) + self.desired_ee_pose_b[:, :3], self.desired_ee_pose_b[:, 3:] = combine_frame_transforms( + current_task_frame_pose_b[:, :3], + current_task_frame_pose_b[:, 3:], + self.desired_ee_pose_task[:, :3], + self.desired_ee_pose_task[:, 3:], + ) + + # Transform desired wrenches to root frame + if self.desired_ee_wrench_task is not None: + self.desired_ee_wrench_b = torch.zeros_like(self.desired_ee_wrench_task) + self.desired_ee_wrench_b[:, :3] = (R_task_b @ self.desired_ee_wrench_task[:, :3].unsqueeze(-1)).squeeze(-1) + self.desired_ee_wrench_b[:, 3:] = (R_task_b @ self.desired_ee_wrench_task[:, 3:].unsqueeze(-1)).squeeze( + -1 + ) + torch.cross(current_task_frame_pose_b[:, :3], self.desired_ee_wrench_b[:, :3], dim=-1) + + def compute( + self, + jacobian_b: torch.Tensor, + current_ee_pose_b: torch.Tensor | None = None, + current_ee_vel_b: torch.Tensor | None = None, + current_ee_force_b: torch.Tensor | None = None, + mass_matrix: torch.Tensor | None = None, + gravity: torch.Tensor | None = None, + current_joint_pos: torch.Tensor | None = None, + current_joint_vel: torch.Tensor | None = None, + nullspace_joint_pos_target: torch.Tensor | None = None, + ) -> torch.Tensor: + """Performs inference with the controller. + + Args: + jacobian_b: The Jacobian matrix of the end-effector in root frame. It is a tensor of shape + (``num_envs``, 6, ``num_DoF``). + current_ee_pose_b: The current end-effector pose in root frame. It is a tensor of shape + (``num_envs``, 7), which contains the position and quaternion ``(w, x, y, z)``. Defaults to ``None``. + current_ee_vel_b: The current end-effector velocity in root frame. It is a tensor of shape + (``num_envs``, 6), which contains the linear and angular velocities. Defaults to None. + current_ee_force_b: The current external force on the end-effector in root frame. It is a tensor of + shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``. + mass_matrix: The joint-space mass/inertia matrix. It is a tensor of shape (``num_envs``, ``num_DoF``, + ``num_DoF``). Defaults to ``None``. + gravity: The joint-space gravity vector. It is a tensor of shape (``num_envs``, ``num_DoF``). Defaults + to ``None``. + current_joint_pos: The current joint positions. It is a tensor of shape (``num_envs``, ``num_DoF``). + Defaults to ``None``. + current_joint_vel: The current joint velocities. It is a tensor of shape (``num_envs``, ``num_DoF``). + Defaults to ``None``. + nullspace_joint_pos_target: The target joint positions the null space controller is trying to enforce, when + possible. It is a tensor of shape (``num_envs``, ``num_DoF``). + + Raises: + ValueError: When motion-control is enabled but the current end-effector pose or velocity is not provided. + ValueError: When inertial dynamics decoupling is enabled but the mass matrix is not provided. + ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command. + ValueError: When closed-loop force control is enabled but the current end-effector force is not provided. + ValueError: When gravity compensation is enabled but the gravity vector is not provided. + ValueError: When null-space control is enabled but the system is not redundant. + ValueError: When dynamically consistent pseudo-inverse is enabled but the mass matrix inverse is not + provided. + ValueError: When null-space control is enabled but the current joint positions and velocities are not + provided. + ValueError: When target joint positions are provided for null-space control but their dimensions do not + match the current joint positions. + ValueError: When an invalid null-space control method is provided. + + Returns: + Tensor: The joint efforts computed by the controller. It is a tensor of shape (``num_envs``, ``num_DoF``). + """ + + # deduce number of DoF + num_DoF = jacobian_b.shape[2] + # create joint effort vector + joint_efforts = torch.zeros(self.num_envs, num_DoF, device=self._device) + + # compute joint efforts for motion-control + if self.desired_ee_pose_b is not None: + # check input is provided + if current_ee_pose_b is None or current_ee_vel_b is None: + raise ValueError("Current end-effector pose and velocity are required for motion control.") + # -- end-effector tracking error + pose_error_b = torch.cat( + compute_pose_error( + current_ee_pose_b[:, :3], + current_ee_pose_b[:, 3:], + self.desired_ee_pose_b[:, :3], + self.desired_ee_pose_b[:, 3:], + rot_error_type="axis_angle", + ), + dim=-1, + ) + velocity_error_b = -current_ee_vel_b # zero target velocity. The target is assumed to be stationary. + # -- desired end-effector acceleration (spring-damper system) + des_ee_acc_b = self._motion_p_gains_b @ pose_error_b.unsqueeze( + -1 + ) + self._motion_d_gains_b @ velocity_error_b.unsqueeze(-1) + # -- Inertial dynamics decoupling + if self.cfg.inertial_dynamics_decoupling: + # check input is provided + if mass_matrix is None: + raise ValueError("Mass matrix is required for inertial decoupling.") + # Compute operational space mass matrix + self._mass_matrix_inv = torch.inverse(mass_matrix) + if self.cfg.partial_inertial_dynamics_decoupling: + # Fill in the translational and rotational parts of the inertia separately, ignoring their coupling + self._os_mass_matrix_b[:, 0:3, 0:3] = torch.inverse( + jacobian_b[:, 0:3] @ self._mass_matrix_inv @ jacobian_b[:, 0:3].mT + ) + self._os_mass_matrix_b[:, 3:6, 3:6] = torch.inverse( + jacobian_b[:, 3:6] @ self._mass_matrix_inv @ jacobian_b[:, 3:6].mT + ) + else: + # Calculate the operational space mass matrix fully accounting for the couplings + self._os_mass_matrix_b[:] = torch.inverse(jacobian_b @ self._mass_matrix_inv @ jacobian_b.mT) + # (Generalized) operational space command forces + # F = (J M^(-1) J^T)^(-1) * \ddot(x_des) = M_task * \ddot(x_des) + os_command_forces_b = self._os_mass_matrix_b @ des_ee_acc_b + else: + # Task-space impedance control: command forces = \ddot(x_des). + # Please note that the definition of task-space impedance control varies in literature. + # This implementation ignores the inertial term. For inertial decoupling, + # use inertial_dynamics_decoupling=True. + os_command_forces_b = des_ee_acc_b + # -- joint-space commands + joint_efforts += (jacobian_b.mT @ self._selection_matrix_motion_b @ os_command_forces_b).squeeze(-1) + + # compute joint efforts for contact wrench/force control + if self.desired_ee_wrench_b is not None: + # -- task-space contact wrench + if self.cfg.contact_wrench_stiffness_task is not None: + # check input is provided + if current_ee_force_b is None: + raise ValueError("Current end-effector force is required for closed-loop force control.") + # We can only measure the force component at the contact, so only apply the feedback for only the force + # component, keep the control of moment components open loop + self._ee_contact_wrench_b[:, 0:3] = current_ee_force_b + self._ee_contact_wrench_b[:, 3:6] = self.desired_ee_wrench_b[:, 3:6] + # closed-loop control with feedforward term + os_contact_wrench_command_b = self.desired_ee_wrench_b.unsqueeze( + -1 + ) + self._contact_wrench_p_gains_b @ (self.desired_ee_wrench_b - self._ee_contact_wrench_b).unsqueeze( + -1 + ) + else: + # open-loop control + os_contact_wrench_command_b = self.desired_ee_wrench_b.unsqueeze(-1) + # -- joint-space commands + joint_efforts += (jacobian_b.mT @ self._selection_matrix_force_b @ os_contact_wrench_command_b).squeeze(-1) + + # add gravity compensation (bias correction) + if self.cfg.gravity_compensation: + # check input is provided + if gravity is None: + raise ValueError("Gravity vector is required for gravity compensation.") + # add gravity compensation + joint_efforts += gravity + + # Add null-space control + # -- Free null-space control + if self.cfg.nullspace_control == "none": + # No additional control is applied in the null space. + pass + else: + # Check if the system is redundant + if num_DoF <= 6: + raise ValueError("Null-space control is only applicable for redundant manipulators.") + + # Calculate the pseudo-inverse of the Jacobian + if self.cfg.inertial_dynamics_decoupling and not self.cfg.partial_inertial_dynamics_decoupling: + # Dynamically consistent pseudo-inverse allows decoupling of null space and task space + if self._mass_matrix_inv is None or mass_matrix is None: + raise ValueError("Mass matrix inverse is required for dynamically consistent pseudo-inverse") + jacobian_pinv_transpose = self._os_mass_matrix_b @ jacobian_b @ self._mass_matrix_inv + else: + # Moore-Penrose pseudo-inverse if full inertia matrix is not available (e.g., no/partial decoupling) + jacobian_pinv_transpose = torch.pinverse(jacobian_b).mT + + # Calculate the null-space projector + nullspace_jacobian_transpose = ( + torch.eye(n=num_DoF, device=self._device) - jacobian_b.mT @ jacobian_pinv_transpose + ) + + # Null space position control + if self.cfg.nullspace_control == "position": + + # Check if the current joint positions and velocities are provided + if current_joint_pos is None or current_joint_vel is None: + raise ValueError("Current joint positions and velocities are required for null-space control.") + + # Calculate the joint errors for nullspace position control + if nullspace_joint_pos_target is None: + nullspace_joint_pos_target = torch.zeros_like(current_joint_pos) + # Check if the dimensions of the target nullspace joint positions match the current joint positions + elif nullspace_joint_pos_target.shape != current_joint_pos.shape: + raise ValueError( + f"The target nullspace joint positions shape '{nullspace_joint_pos_target.shape}' does not" + f"match the current joint positions shape '{current_joint_pos.shape}'." + ) + + joint_pos_error_nullspace = nullspace_joint_pos_target - current_joint_pos + joint_vel_error_nullspace = -current_joint_vel + + # Calculate the desired joint accelerations + joint_acc_nullspace = ( + self._nullspace_p_gain * joint_pos_error_nullspace + + self._nullspace_d_gain * joint_vel_error_nullspace + ).unsqueeze(-1) + + # Calculate the projected torques in null-space + if mass_matrix is not None: + tau_null = (nullspace_jacobian_transpose @ mass_matrix @ joint_acc_nullspace).squeeze(-1) + else: + tau_null = nullspace_jacobian_transpose @ joint_acc_nullspace + + # Add the null-space joint efforts to the total joint efforts + joint_efforts += tau_null + + else: + raise ValueError(f"Invalid null-space control method: {self.cfg.nullspace_control}.") + + return joint_efforts diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space_cfg.py new file mode 100644 index 00000000000..5983a3ec1e7 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/operational_space_cfg.py @@ -0,0 +1,95 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Sequence +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .operational_space import OperationalSpaceController + + +@configclass +class OperationalSpaceControllerCfg: + """Configuration for operational-space controller.""" + + class_type: type = OperationalSpaceController + """The associated controller class.""" + + target_types: Sequence[str] = MISSING + """Type of task-space targets. + + It has two sub-strings joined by underscore: + - type of task-space target: ``"pose"``, ``"wrench"`` + - reference for the task-space targets: ``"abs"`` (absolute), ``"rel"`` (relative, only for pose) + """ + + motion_control_axes_task: Sequence[int] = (1, 1, 1, 1, 1, 1) + """Motion direction to control in task reference frame. Mark as ``0/1`` for each axis.""" + + contact_wrench_control_axes_task: Sequence[int] = (0, 0, 0, 0, 0, 0) + """Contact wrench direction to control in task reference frame. Mark as 0/1 for each axis.""" + + inertial_dynamics_decoupling: bool = False + """Whether to perform inertial dynamics decoupling for motion control (inverse dynamics).""" + + partial_inertial_dynamics_decoupling: bool = False + """Whether to ignore the inertial coupling between the translational & rotational motions.""" + + gravity_compensation: bool = False + """Whether to perform gravity compensation.""" + + impedance_mode: str = "fixed" + """Type of gains for motion control: ``"fixed"``, ``"variable"``, ``"variable_kp"``.""" + + motion_stiffness_task: float | Sequence[float] = (100.0, 100.0, 100.0, 100.0, 100.0, 100.0) + """The positional gain for determining operational space command forces based on task-space pose error.""" + + motion_damping_ratio_task: float | Sequence[float] = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + """The damping ratio is used in-conjunction with positional gain to compute operational space command forces + based on task-space velocity error. + + The following math operation is performed for computing velocity gains: + :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. + """ + + motion_stiffness_limits_task: tuple[float, float] = (0, 1000) + """Minimum and maximum values for positional gains. + + Note: Used only when :obj:`impedance_mode` is ``"variable"`` or ``"variable_kp"``. + """ + + motion_damping_ratio_limits_task: tuple[float, float] = (0, 100) + """Minimum and maximum values for damping ratios used to compute velocity gains. + + Note: Used only when :obj:`impedance_mode` is ``"variable"``. + """ + + contact_wrench_stiffness_task: float | Sequence[float] | None = None + """The proportional gain for determining operational space command forces for closed-loop contact force control. + + If ``None``, then open-loop control of desired contact wrench is performed. + + Note: since only the linear forces could be measured at the moment, + only the first three elements are used for the feedback loop. + """ + + nullspace_control: str = "none" + """The null space control method for redundant manipulators: ``"none"``, ``"position"``. + + Note: ``"position"`` is used to drive the redundant manipulator to zero configuration by default. If + ``target_joint_pos`` is provided in the ``compute()`` method, it will be driven to this configuration. + """ + + nullspace_stiffness: float = 10.0 + """The stiffness for null space control.""" + + nullspace_damping_ratio: float = 1.0 + """The damping ratio for null space control.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik.py new file mode 100644 index 00000000000..3657fa6a0fe --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik.py @@ -0,0 +1,138 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pink IK controller implementation for IsaacLab. + +This module provides integration between Pink inverse kinematics solver and IsaacLab. +Pink is a differentiable inverse kinematics solver framework that provides task-space control capabilities. +""" + +import numpy as np +import torch + +from pink import solve_ik +from pink.configuration import Configuration +from pinocchio.robot_wrapper import RobotWrapper + +from .pink_ik_cfg import PinkIKControllerCfg + + +class PinkIKController: + """Integration of Pink IK controller with Isaac Lab. + + The Pink IK controller is available at: https://github.com/stephane-caron/pink + """ + + def __init__(self, cfg: PinkIKControllerCfg, device: str): + """Initialize the Pink IK Controller. + + Args: + cfg: The configuration for the controller. + device: The device to use for computations (e.g., 'cuda:0'). + """ + # Initialize the robot model from URDF and mesh files + self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None) + self.pink_configuration = Configuration( + self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0 + ) + + # Set the default targets for each task from the configuration + for task in cfg.variable_input_tasks: + task.set_target_from_configuration(self.pink_configuration) + for task in cfg.fixed_input_tasks: + task.set_target_from_configuration(self.pink_configuration) + + # Map joint names from Isaac Lab to Pink's joint conventions + pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints + isaac_lab_joint_names = cfg.joint_names + + # Create reordering arrays for joint indices + self.isaac_lab_to_pink_ordering = [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names] + self.pink_to_isaac_lab_ordering = [ + pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names + ] + + self.cfg = cfg + self.device = device + + """ + Operations. + """ + + def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: + """Reorder the input array based on the provided ordering. + + Args: + input_array: The array to reorder. + reordering_array: The indices to use for reordering. + + Returns: + Reordered array. + """ + return [input_array[i] for i in reordering_array] + + def initialize(self): + """Initialize the internals of the controller. + + This method is called during setup but before the first compute call. + """ + pass + + def compute( + self, + curr_joint_pos: np.ndarray, + dt: float, + ) -> torch.Tensor: + """Compute the target joint positions based on current state and tasks. + + Args: + curr_joint_pos: The current joint positions. + dt: The time step for computing joint position changes. + + Returns: + The target joint positions as a tensor. + """ + # Initialize joint positions for Pink, including the root and universal joints + joint_positions_pink = np.array(self.reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering)) + + # Update Pink's robot configuration with the current joint positions + self.pink_configuration.update(joint_positions_pink) + + # pink.solve_ik can raise an exception if the solver fails + try: + velocity = solve_ik( + self.pink_configuration, self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, dt, solver="osqp" + ) + Delta_q = velocity * dt + except (AssertionError, Exception): + # Print warning and return the current joint positions as the target + # Not using omni.log since its not available in CI during docs build + if self.cfg.show_ik_warnings: + print( + "Warning: IK quadratic solver could not find a solution! Did not update the target joint positions." + ) + return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) + + # Discard the first 6 values (for root and universal joints) + pink_joint_angle_changes = Delta_q + + # Reorder the joint angle changes back to Isaac Lab conventions + joint_vel_isaac_lab = torch.tensor( + self.reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering), + device=self.device, + dtype=torch.float, + ) + + # Add the velocity changes to the current joint positions to get the target joint positions + target_joint_pos = torch.add( + joint_vel_isaac_lab, torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) + ) + + return target_joint_pos diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik_cfg.py new file mode 100644 index 00000000000..c084a7643e5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/pink_ik_cfg.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Pink IK controller.""" + +from dataclasses import MISSING + +from pink.tasks import FrameTask + +from isaaclab.utils import configclass + + +@configclass +class PinkIKControllerCfg: + """Configuration settings for the Pink IK Controller. + + The Pink IK controller can be found at: https://github.com/stephane-caron/pink + """ + + urdf_path: str | None = None + """Path to the robot's URDF file. This file is used by Pinocchio's `robot_wrapper.BuildFromURDF` to load the robot model.""" + + mesh_path: str | None = None + """Path to the mesh files associated with the robot. These files are also loaded by Pinocchio's `robot_wrapper.BuildFromURDF`.""" + + num_hand_joints: int = 0 + """The number of hand joints in the robot. The action space for the controller contains the pose_dim(7)*num_controlled_frames + num_hand_joints. + The last num_hand_joints values of the action are the hand joint angles.""" + + variable_input_tasks: list[FrameTask] = MISSING + """ + A list of tasks for the Pink IK controller. These tasks are controllable by the env action. + + These tasks can be used to control the pose of a frame or the angles of joints. + For more details, visit: https://github.com/stephane-caron/pink + """ + + fixed_input_tasks: list[FrameTask] = MISSING + """ + A list of tasks for the Pink IK controller. These tasks are fixed and not controllable by the env action. + + These tasks can be used to fix the pose of a frame or the angles of joints to a desired configuration. + For more details, visit: https://github.com/stephane-caron/pink + """ + + joint_names: list[str] | None = None + """A list of joint names in the USD asset. This is required because the joint naming conventions differ between USD and URDF files. + This value is currently designed to be automatically populated by the action term in a manager based environment.""" + + articulation_name: str = "robot" + """The name of the articulation USD asset in the scene.""" + + base_link_name: str = "base_link" + """The name of the base link in the USD asset.""" + + show_ik_warnings: bool = True + """Show warning if IK solver fails to find a solution.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/rmp_flow.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/rmp_flow.py new file mode 100644 index 00000000000..f43ba4ad5c1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/rmp_flow.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import MISSING + +import isaacsim.core.utils.prims as prim_utils +from isaacsim.core.api.simulation_context import SimulationContext +from isaacsim.core.prims import SingleArticulation + +# enable motion generation extensions +from isaacsim.core.utils.extensions import enable_extension + +enable_extension("isaacsim.robot_motion.lula") +enable_extension("isaacsim.robot_motion.motion_generation") + +from isaacsim.robot_motion.motion_generation import ArticulationMotionPolicy +from isaacsim.robot_motion.motion_generation.lula.motion_policies import RmpFlow, RmpFlowSmoothed + +from isaaclab.utils import configclass + + +@configclass +class RmpFlowControllerCfg: + """Configuration for RMP-Flow controller (provided through LULA library).""" + + name: str = "rmp_flow" + """Name of the controller. Supported: "rmp_flow", "rmp_flow_smoothed". Defaults to "rmp_flow".""" + config_file: str = MISSING + """Path to the configuration file for the controller.""" + urdf_file: str = MISSING + """Path to the URDF model of the robot.""" + collision_file: str = MISSING + """Path to collision model description of the robot.""" + frame_name: str = MISSING + """Name of the robot frame for task space (must be present in the URDF).""" + evaluations_per_frame: float = MISSING + """Number of substeps during Euler integration inside LULA world model.""" + ignore_robot_state_updates: bool = False + """If true, then state of the world model inside controller is rolled out. Defaults to False.""" + + +class RmpFlowController: + """Wraps around RMPFlow from IsaacSim for batched environments.""" + + def __init__(self, cfg: RmpFlowControllerCfg, device: str): + """Initialize the controller. + + Args: + cfg: The configuration for the controller. + device: The device to use for computation. + """ + # store input + self.cfg = cfg + self._device = device + # display info + print(f"[INFO]: Loading RMPFlow controller URDF from: {self.cfg.urdf_file}") + + """ + Properties. + """ + + @property + def num_actions(self) -> int: + """Dimension of the action space of controller.""" + return 7 + + """ + Operations. + """ + + def initialize(self, prim_paths_expr: str): + """Initialize the controller. + + Args: + prim_paths_expr: The expression to find the articulation prim paths. + """ + # obtain the simulation time + physics_dt = SimulationContext.instance().get_physics_dt() + # find all prims + self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr) + self.num_robots = len(self._prim_paths) + # resolve controller + if self.cfg.name == "rmp_flow": + controller_cls = RmpFlow + elif self.cfg.name == "rmp_flow_smoothed": + controller_cls = RmpFlowSmoothed + else: + raise ValueError(f"Unsupported controller in Lula library: {self.cfg.name}") + # create all franka robots references and their controllers + self.articulation_policies = list() + for prim_path in self._prim_paths: + # add robot reference + robot = SingleArticulation(prim_path) + robot.initialize() + # add controller + rmpflow = controller_cls( + robot_description_path=self.cfg.collision_file, + urdf_path=self.cfg.urdf_file, + rmpflow_config_path=self.cfg.config_file, + end_effector_frame_name=self.cfg.frame_name, + maximum_substep_size=physics_dt / self.cfg.evaluations_per_frame, + ignore_robot_state_updates=self.cfg.ignore_robot_state_updates, + ) + # wrap rmpflow to connect to the Franka robot articulation + articulation_policy = ArticulationMotionPolicy(robot, rmpflow, physics_dt) + self.articulation_policies.append(articulation_policy) + # get number of active joints + self.active_dof_names = self.articulation_policies[0].get_motion_policy().get_active_joints() + self.num_dof = len(self.active_dof_names) + # create buffers + # -- for storing command + self._command = torch.zeros(self.num_robots, self.num_actions, device=self._device) + # -- for policy output + self.dof_pos_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) + self.dof_vel_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) + + def reset_idx(self, robot_ids: torch.Tensor = None): + """Reset the internals.""" + # if no robot ids are provided, then reset all robots + if robot_ids is None: + robot_ids = torch.arange(self.num_robots, device=self._device) + # reset policies for specified robots + for index in robot_ids: + self.articulation_policies[index].motion_policy.reset() + + def set_command(self, command: torch.Tensor): + """Set target end-effector pose command.""" + # store command + self._command[:] = command + + def compute(self) -> tuple[torch.Tensor, torch.Tensor]: + """Performs inference with the controller. + + Returns: + The target joint positions and velocity commands. + """ + # convert command to numpy + command = self._command.cpu().numpy() + # compute control actions + for i, policy in enumerate(self.articulation_policies): + # enable type-hinting + policy: ArticulationMotionPolicy + # set rmpflow target to be the current position of the target cube. + policy.get_motion_policy().set_end_effector_target( + target_position=command[i, 0:3], target_orientation=command[i, 3:7] + ) + # apply action on the robot + action = policy.get_next_articulation_action() + # copy actions into buffer + self.dof_pos_target[i, :] = torch.from_numpy(action.joint_positions[:]).to(self.dof_pos_target) + self.dof_vel_target[i, :] = torch.from_numpy(action.joint_velocities[:]).to(self.dof_vel_target) + + return self.dof_pos_target, self.dof_vel_target diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/utils.py new file mode 100644 index 00000000000..3e274011d11 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/controllers/utils.py @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions for Isaac Lab controllers. + +This module provides utility functions to help with controller implementations. +""" + +import os + +from isaacsim.core.utils.extensions import enable_extension + +enable_extension("isaacsim.asset.exporter.urdf") + +import nvidia.srl.tools.logger as logger +import omni.log +from nvidia.srl.from_usd.to_urdf import UsdToUrdf + + +def convert_usd_to_urdf(usd_path: str, output_path: str, force_conversion: bool = True) -> tuple[str, str]: + """Convert a USD file to URDF format. + + Args: + usd_path: Path to the USD file to convert. + output_path: Directory to save the converted URDF and mesh files. + force_conversion: Whether to force the conversion even if the URDF and mesh files already exist. + Returns: + A tuple containing the paths to the URDF file and the mesh directory. + """ + usd_to_urdf_kwargs = { + "node_names_to_remove": None, + "edge_names_to_remove": None, + "root": None, + "parent_link_is_body_1": None, + "log_level": logger.level_from_name("ERROR"), + } + + urdf_output_dir = os.path.join(output_path, "urdf") + urdf_file_name = os.path.basename(usd_path).split(".")[0] + ".urdf" + urdf_output_path = urdf_output_dir + "/" + urdf_file_name + urdf_meshes_output_dir = os.path.join(output_path, "meshes") + + if not os.path.exists(urdf_output_path) or not os.path.exists(urdf_meshes_output_dir) or force_conversion: + usd_to_urdf = UsdToUrdf.init_from_file(usd_path, **usd_to_urdf_kwargs) + os.makedirs(urdf_output_dir, exist_ok=True) + os.makedirs(urdf_meshes_output_dir, exist_ok=True) + + output_path = usd_to_urdf.save_to_file( + urdf_output_path=urdf_output_path, + visualize_collision_meshes=False, + mesh_dir=urdf_meshes_output_dir, + mesh_path_prefix="", + ) + + # The current version of the usd to urdf converter creates "inf" effort, + # This has to be replaced with a max value for the urdf to be valid + # Open the file for reading and writing + with open(urdf_output_path) as file: + # Read the content of the file + content = file.read() + + # Replace all occurrences of 'inf' with '0' + content = content.replace("inf", "0.") + + # Open the file again to write the modified content + with open(urdf_output_path, "w") as file: + # Write the modified content back to the file + file.write(content) + return urdf_output_path, urdf_meshes_output_dir + + +def change_revolute_to_fixed(urdf_path: str, fixed_joints: list[str], verbose: bool = False): + """Change revolute joints to fixed joints in a URDF file. + + This function modifies a URDF file by changing specified revolute joints to fixed joints. + This is useful when you want to disable certain joints in a robot model. + + Args: + urdf_path: Path to the URDF file to modify. + fixed_joints: List of joint names to convert from revolute to fixed. + verbose: Whether to print information about the changes being made. + """ + with open(urdf_path) as file: + content = file.read() + + for joint in fixed_joints: + old_str = f'' + new_str = f'' + if verbose: + omni.log.warn(f"Replacing {joint} with fixed joint") + omni.log.warn(old_str) + omni.log.warn(new_str) + if old_str not in content: + omni.log.warn(f"Error: Could not find revolute joint named '{joint}' in URDF file") + content = content.replace(old_str, new_str) + + with open(urdf_path, "w") as file: + file.write(content) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/__init__.py new file mode 100644 index 00000000000..712fc1de6da --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/__init__.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package providing interfaces to different teleoperation devices. + +Currently, the following categories of devices are supported: + +* **Keyboard**: Standard keyboard with WASD and arrow keys. +* **Spacemouse**: 3D mouse with 6 degrees of freedom. +* **Gamepad**: Gamepad with 2D two joysticks and buttons. Example: Xbox controller. +* **OpenXR**: Uses hand tracking of index/thumb tip avg to drive the target pose. Gripping is done with pinching. + +All device interfaces inherit from the :class:`DeviceBase` class, which provides a +common interface for all devices. The device interface reads the input data when +the :meth:`DeviceBase.advance` method is called. It also provides the function :meth:`DeviceBase.add_callback` +to add user-defined callback functions to be called when a particular input is pressed from +the peripheral device. +""" + +from .device_base import DeviceBase +from .gamepad import Se2Gamepad, Se3Gamepad +from .keyboard import Se2Keyboard, Se3Keyboard +from .openxr import OpenXRDevice +from .retargeter_base import RetargeterBase +from .spacemouse import Se2SpaceMouse, Se3SpaceMouse diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/device_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/device_base.py new file mode 100644 index 00000000000..f3db507d783 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/device_base.py @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base class for teleoperation interface.""" + +from abc import ABC, abstractmethod +from collections.abc import Callable +from typing import Any + +from isaaclab.devices.retargeter_base import RetargeterBase + + +class DeviceBase(ABC): + """An interface class for teleoperation devices. + + Derived classes have two implementation options: + + 1. Override _get_raw_data() and use the base advance() implementation: + This approach is suitable for devices that want to leverage the built-in + retargeting logic but only need to customize the raw data acquisition. + + 2. Override advance() completely: + This approach gives full control over the command generation process, + and _get_raw_data() can be ignored entirely. + """ + + def __init__(self, retargeters: list[RetargeterBase] | None = None): + """Initialize the teleoperation interface. + + Args: + retargeters: List of components that transform device data into robot commands. + If None or empty list, the device will output its native data format. + """ + # Initialize empty list if None is provided + self._retargeters = retargeters or [] + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + return f"{self.__class__.__name__}" + + """ + Operations + """ + + @abstractmethod + def reset(self): + """Reset the internals.""" + raise NotImplementedError + + @abstractmethod + def add_callback(self, key: Any, func: Callable): + """Add additional functions to bind keyboard. + + Args: + key: The button to check against. + func: The function to call when key is pressed. The callback function should not + take any arguments. + """ + raise NotImplementedError + + def _get_raw_data(self) -> Any: + """Internal method to get the raw data from the device. + + This method is intended for internal use by the advance() implementation. + Derived classes can override this method to customize raw data acquisition + while still using the base class's advance() implementation. + + Returns: + Raw device data in a device-specific format + + Note: + This is an internal implementation detail. Clients should call advance() + instead of this method. + """ + raise NotImplementedError("Derived class must implement _get_raw_data() or override advance()") + + def advance(self) -> Any: + """Process current device state and return control commands. + + This method retrieves raw data from the device and optionally applies + retargeting to convert it to robot commands. + + Derived classes can either: + 1. Override _get_raw_data() and use this base implementation, or + 2. Override this method completely for custom command processing + + Returns: + Raw device data if no retargeters are configured. + When retargeters are configured, returns a tuple containing each retargeter's processed output. + """ + raw_data = self._get_raw_data() + + # If no retargeters, return raw data directly (not as a tuple) + if not self._retargeters: + return raw_data + + # With multiple retargeters, return a tuple of outputs + return tuple(retargeter.retarget(raw_data) for retargeter in self._retargeters) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/__init__.py new file mode 100644 index 00000000000..8388f7d542e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Gamepad device for SE(2) and SE(3) control.""" + +from .se2_gamepad import Se2Gamepad +from .se3_gamepad import Se3Gamepad diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se2_gamepad.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se2_gamepad.py new file mode 100644 index 00000000000..97b153955a9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se2_gamepad.py @@ -0,0 +1,205 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Gamepad controller for SE(2) control.""" + +import numpy as np +import weakref +from collections.abc import Callable + +import carb +import carb.input +import omni + +from ..device_base import DeviceBase + + +class Se2Gamepad(DeviceBase): + r"""A gamepad controller for sending SE(2) commands as velocity commands. + + This class is designed to provide a gamepad controller for mobile base (such as quadrupeds). + It uses the Omniverse gamepad interface to listen to gamepad events and map them to robot's + task-space commands. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`. + + Key bindings: + ====================== ========================= ======================== + Command Key (+ve axis) Key (-ve axis) + ====================== ========================= ======================== + Move along x-axis left stick up left stick down + Move along y-axis left stick right left stick left + Rotate along z-axis right stick right right stick left + ====================== ========================= ======================== + + .. seealso:: + + The official documentation for the gamepad interface: `Carb Gamepad Interface `__. + + """ + + def __init__( + self, + v_x_sensitivity: float = 1.0, + v_y_sensitivity: float = 1.0, + omega_z_sensitivity: float = 1.0, + dead_zone: float = 0.01, + ): + """Initialize the gamepad layer. + + Args: + v_x_sensitivity: Magnitude of linear velocity along x-direction scaling. Defaults to 1.0. + v_y_sensitivity: Magnitude of linear velocity along y-direction scaling. Defaults to 1.0. + omega_z_sensitivity: Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + dead_zone: Magnitude of dead zone for gamepad. An event value from the gamepad less than + this value will be ignored. Defaults to 0.01. + """ + # turn off simulator gamepad control + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/persistent/app/omniverse/gamepadCameraControl", False) + # store inputs + self.v_x_sensitivity = v_x_sensitivity + self.v_y_sensitivity = v_y_sensitivity + self.omega_z_sensitivity = omega_z_sensitivity + self.dead_zone = dead_zone + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._gamepad = self._appwindow.get_gamepad(0) + # note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called + self._gamepad_sub = self._input.subscribe_to_gamepad_events( + self._gamepad, + lambda event, *args, obj=weakref.proxy(self): obj._on_gamepad_event(event, *args), + ) + # bindings for gamepad to command + self._create_key_bindings() + # command buffers + # When using the gamepad, two values are provided for each axis. + # For example: when the left stick is moved down, there are two evens: `left_stick_down = 0.8` + # and `left_stick_up = 0.0`. If only the value of left_stick_up is used, the value will be 0.0, + # which is not the desired behavior. Therefore, we save both the values into the buffer and use + # the maximum value. + # (positive, negative), (x, y, yaw) + self._base_command_raw = np.zeros([2, 3]) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __del__(self): + """Unsubscribe from gamepad events.""" + self._input.unsubscribe_to_gamepad_events(self._gamepad, self._gamepad_sub) + self._gamepad_sub = None + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Gamepad Controller for SE(2): {self.__class__.__name__}\n" + msg += f"\tDevice name: {self._input.get_gamepad_name(self._gamepad)}\n" + msg += "\t----------------------------------------------\n" + msg += "\tMove in X-Y plane: left stick\n" + msg += "\tRotate in Z-axis: right stick\n" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._base_command_raw.fill(0.0) + + def add_callback(self, key: carb.input.GamepadInput, func: Callable): + """Add additional functions to bind gamepad. + + A list of available gamepad keys are present in the + `carb documentation `__. + + Args: + key: The gamepad button to check against. + func: The function to call when key is pressed. The callback function should not + take any arguments. + """ + self._additional_callbacks[key] = func + + def advance(self) -> np.ndarray: + """Provides the result from gamepad event state. + + Returns: + A 3D array containing the linear (x,y) and angular velocity (z). + """ + return self._resolve_command_buffer(self._base_command_raw) + + """ + Internal helpers. + """ + + def _on_gamepad_event(self, event: carb.input.GamepadEvent, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/dev-guide/latest/programmer_ref/input-devices/gamepad.html + """ + + # check if the event is a button press + cur_val = event.value + if abs(cur_val) < self.dead_zone: + cur_val = 0 + # -- left and right stick + if event.input in self._INPUT_STICK_VALUE_MAPPING: + direction, axis, value = self._INPUT_STICK_VALUE_MAPPING[event.input] + # change the value only if the stick is moved (soft press) + self._base_command_raw[direction, axis] = value * cur_val + + # additional callbacks + if event.input in self._additional_callbacks: + self._additional_callbacks[event.input]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + self._INPUT_STICK_VALUE_MAPPING = { + # forward command + carb.input.GamepadInput.LEFT_STICK_UP: (0, 0, self.v_x_sensitivity), + # backward command + carb.input.GamepadInput.LEFT_STICK_DOWN: (1, 0, self.v_x_sensitivity), + # right command + carb.input.GamepadInput.LEFT_STICK_RIGHT: (0, 1, self.v_y_sensitivity), + # left command + carb.input.GamepadInput.LEFT_STICK_LEFT: (1, 1, self.v_y_sensitivity), + # yaw command (positive) + carb.input.GamepadInput.RIGHT_STICK_RIGHT: (0, 2, self.omega_z_sensitivity), + # yaw command (negative) + carb.input.GamepadInput.RIGHT_STICK_LEFT: (1, 2, self.omega_z_sensitivity), + } + + def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: + """Resolves the command buffer. + + Args: + raw_command: The raw command from the gamepad. Shape is (2, 3) + This is a 2D array since gamepad dpad/stick returns two values corresponding to + the positive and negative direction. The first index is the direction (0: positive, 1: negative) + and the second index is value (absolute) of the command. + + Returns: + Resolved command. Shape is (3,) + """ + # compare the positive and negative value decide the sign of the value + # if the positive value is larger, the sign is positive (i.e. False, 0) + # if the negative value is larger, the sign is positive (i.e. True, 1) + command_sign = raw_command[1, :] > raw_command[0, :] + # extract the command value + command = raw_command.max(axis=0) + # apply the sign + # if the sign is positive, the value is already positive. + # if the sign is negative, the value is negative after applying the sign. + command[command_sign] *= -1 + + return command diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se3_gamepad.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se3_gamepad.py new file mode 100644 index 00000000000..a40121227eb --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/gamepad/se3_gamepad.py @@ -0,0 +1,249 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Gamepad controller for SE(3) control.""" + +import numpy as np +import weakref +from collections.abc import Callable +from scipy.spatial.transform import Rotation + +import carb +import omni + +from ..device_base import DeviceBase + + +class Se3Gamepad(DeviceBase): + """A gamepad controller for sending SE(3) commands as delta poses and binary command (open/close). + + This class is designed to provide a gamepad controller for a robotic arm with a gripper. + It uses the gamepad interface to listen to gamepad events and map them to the robot's + task-space commands. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Stick and Button bindings: + ============================ ========================= ========================= + Description Stick/Button (+ve axis) Stick/Button (-ve axis) + ============================ ========================= ========================= + Toggle gripper(open/close) X Button X Button + Move along x-axis Left Stick Up Left Stick Down + Move along y-axis Left Stick Left Left Stick Right + Move along z-axis Right Stick Up Right Stick Down + Rotate along x-axis D-Pad Left D-Pad Right + Rotate along y-axis D-Pad Down D-Pad Up + Rotate along z-axis Right Stick Left Right Stick Right + ============================ ========================= ========================= + + .. seealso:: + + The official documentation for the gamepad interface: `Carb Gamepad Interface `__. + + """ + + def __init__(self, pos_sensitivity: float = 1.0, rot_sensitivity: float = 1.6, dead_zone: float = 0.01): + """Initialize the gamepad layer. + + Args: + pos_sensitivity: Magnitude of input position command scaling. Defaults to 1.0. + rot_sensitivity: Magnitude of scale input rotation commands scaling. Defaults to 1.6. + dead_zone: Magnitude of dead zone for gamepad. An event value from the gamepad less than + this value will be ignored. Defaults to 0.01. + """ + # turn off simulator gamepad control + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/persistent/app/omniverse/gamepadCameraControl", False) + # store inputs + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + self.dead_zone = dead_zone + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._gamepad = self._appwindow.get_gamepad(0) + # note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called + self._gamepad_sub = self._input.subscribe_to_gamepad_events( + self._gamepad, + lambda event, *args, obj=weakref.proxy(self): obj._on_gamepad_event(event, *args), + ) + # bindings for gamepad to command + self._create_key_bindings() + # command buffers + self._close_gripper = False + # When using the gamepad, two values are provided for each axis. + # For example: when the left stick is moved down, there are two evens: `left_stick_down = 0.8` + # and `left_stick_up = 0.0`. If only the value of left_stick_up is used, the value will be 0.0, + # which is not the desired behavior. Therefore, we save both the values into the buffer and use + # the maximum value. + # (positive, negative), (x, y, z, roll, pitch, yaw) + self._delta_pose_raw = np.zeros([2, 6]) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __del__(self): + """Unsubscribe from gamepad events.""" + self._input.unsubscribe_to_gamepad_events(self._gamepad, self._gamepad_sub) + self._gamepad_sub = None + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Gamepad Controller for SE(3): {self.__class__.__name__}\n" + msg += f"\tDevice name: {self._input.get_gamepad_name(self._gamepad)}\n" + msg += "\t----------------------------------------------\n" + msg += "\tToggle gripper (open/close): X\n" + msg += "\tMove arm along x-axis: Left Stick Up/Down\n" + msg += "\tMove arm along y-axis: Left Stick Left/Right\n" + msg += "\tMove arm along z-axis: Right Stick Up/Down\n" + msg += "\tRotate arm along x-axis: D-Pad Right/Left\n" + msg += "\tRotate arm along y-axis: D-Pad Down/Up\n" + msg += "\tRotate arm along z-axis: Right Stick Left/Right\n" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._close_gripper = False + self._delta_pose_raw.fill(0.0) + + def add_callback(self, key: carb.input.GamepadInput, func: Callable): + """Add additional functions to bind gamepad. + + A list of available gamepad keys are present in the + `carb documentation `__. + + Args: + key: The gamepad button to check against. + func: The function to call when key is pressed. The callback function should not + take any arguments. + """ + self._additional_callbacks[key] = func + + def advance(self) -> tuple[np.ndarray, bool]: + """Provides the result from gamepad event state. + + Returns: + A tuple containing the delta pose command and gripper commands. + """ + # -- resolve position command + delta_pos = self._resolve_command_buffer(self._delta_pose_raw[:, :3]) + # -- resolve rotation command + delta_rot = self._resolve_command_buffer(self._delta_pose_raw[:, 3:]) + # -- convert to rotation vector + rot_vec = Rotation.from_euler("XYZ", delta_rot).as_rotvec() + # return the command and gripper state + return np.concatenate([delta_pos, rot_vec]), self._close_gripper + + """ + Internal helpers. + """ + + def _on_gamepad_event(self, event, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/dev-guide/latest/programmer_ref/input-devices/gamepad.html + """ + # check if the event is a button press + cur_val = event.value + if abs(cur_val) < self.dead_zone: + cur_val = 0 + # -- button + if event.input == carb.input.GamepadInput.X: + # toggle gripper based on the button pressed + if cur_val > 0.5: + self._close_gripper = not self._close_gripper + # -- left and right stick + if event.input in self._INPUT_STICK_VALUE_MAPPING: + direction, axis, value = self._INPUT_STICK_VALUE_MAPPING[event.input] + # change the value only if the stick is moved (soft press) + self._delta_pose_raw[direction, axis] = value * cur_val + # -- dpad (4 arrow buttons on the console) + if event.input in self._INPUT_DPAD_VALUE_MAPPING: + direction, axis, value = self._INPUT_DPAD_VALUE_MAPPING[event.input] + # change the value only if button is pressed on the DPAD + if cur_val > 0.5: + self._delta_pose_raw[direction, axis] = value + self._delta_pose_raw[1 - direction, axis] = 0 + else: + self._delta_pose_raw[:, axis] = 0 + # additional callbacks + if event.input in self._additional_callbacks: + self._additional_callbacks[event.input]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + # map gamepad input to the element in self._delta_pose_raw + # the first index is the direction (0: positive, 1: negative) + # the second index is the axis (0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw) + # the third index is the sensitivity of the command + self._INPUT_STICK_VALUE_MAPPING = { + # forward command + carb.input.GamepadInput.LEFT_STICK_UP: (0, 0, self.pos_sensitivity), + # backward command + carb.input.GamepadInput.LEFT_STICK_DOWN: (1, 0, self.pos_sensitivity), + # right command + carb.input.GamepadInput.LEFT_STICK_RIGHT: (0, 1, self.pos_sensitivity), + # left command + carb.input.GamepadInput.LEFT_STICK_LEFT: (1, 1, self.pos_sensitivity), + # upward command + carb.input.GamepadInput.RIGHT_STICK_UP: (0, 2, self.pos_sensitivity), + # downward command + carb.input.GamepadInput.RIGHT_STICK_DOWN: (1, 2, self.pos_sensitivity), + # yaw command (positive) + carb.input.GamepadInput.RIGHT_STICK_RIGHT: (0, 5, self.rot_sensitivity), + # yaw command (negative) + carb.input.GamepadInput.RIGHT_STICK_LEFT: (1, 5, self.rot_sensitivity), + } + + self._INPUT_DPAD_VALUE_MAPPING = { + # pitch command (positive) + carb.input.GamepadInput.DPAD_UP: (1, 4, self.rot_sensitivity * 0.8), + # pitch command (negative) + carb.input.GamepadInput.DPAD_DOWN: (0, 4, self.rot_sensitivity * 0.8), + # roll command (positive) + carb.input.GamepadInput.DPAD_RIGHT: (1, 3, self.rot_sensitivity * 0.8), + # roll command (negative) + carb.input.GamepadInput.DPAD_LEFT: (0, 3, self.rot_sensitivity * 0.8), + } + + def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: + """Resolves the command buffer. + + Args: + raw_command: The raw command from the gamepad. Shape is (2, 3) + This is a 2D array since gamepad dpad/stick returns two values corresponding to + the positive and negative direction. The first index is the direction (0: positive, 1: negative) + and the second index is value (absolute) of the command. + + Returns: + Resolved command. Shape is (3,) + """ + # compare the positive and negative value decide the sign of the value + # if the positive value is larger, the sign is positive (i.e. False, 0) + # if the negative value is larger, the sign is positive (i.e. True, 1) + delta_command_sign = raw_command[1, :] > raw_command[0, :] + # extract the command value + delta_command = raw_command.max(axis=0) + # apply the sign + # if the sign is positive, the value is already positive. + # if the sign is negative, the value is negative after applying the sign. + delta_command[delta_command_sign] *= -1 + + return delta_command diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/__init__.py new file mode 100644 index 00000000000..2225afe577b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Keyboard device for SE(2) and SE(3) control.""" + +from .se2_keyboard import Se2Keyboard +from .se3_keyboard import Se3Keyboard diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se2_keyboard.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se2_keyboard.py new file mode 100644 index 00000000000..9e21fe28fb9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se2_keyboard.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Keyboard controller for SE(2) control.""" + +import numpy as np +import weakref +from collections.abc import Callable + +import carb +import omni + +from ..device_base import DeviceBase + + +class Se2Keyboard(DeviceBase): + r"""A keyboard controller for sending SE(2) commands as velocity commands. + + This class is designed to provide a keyboard controller for mobile base (such as quadrupeds). + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`. + + Key bindings: + ====================== ========================= ======================== + Command Key (+ve axis) Key (-ve axis) + ====================== ========================= ======================== + Move along x-axis Numpad 8 / Arrow Up Numpad 2 / Arrow Down + Move along y-axis Numpad 4 / Arrow Right Numpad 6 / Arrow Left + Rotate along z-axis Numpad 7 / Z Numpad 9 / X + ====================== ========================= ======================== + + .. seealso:: + + The official documentation for the keyboard interface: `Carb Keyboard Interface `__. + + """ + + def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + """Initialize the keyboard layer. + + Args: + v_x_sensitivity: Magnitude of linear velocity along x-direction scaling. Defaults to 0.8. + v_y_sensitivity: Magnitude of linear velocity along y-direction scaling. Defaults to 0.4. + omega_z_sensitivity: Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + """ + # store inputs + self.v_x_sensitivity = v_x_sensitivity + self.v_y_sensitivity = v_y_sensitivity + self.omega_z_sensitivity = omega_z_sensitivity + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._keyboard = self._appwindow.get_keyboard() + # note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called + self._keyboard_sub = self._input.subscribe_to_keyboard_events( + self._keyboard, + lambda event, *args, obj=weakref.proxy(self): obj._on_keyboard_event(event, *args), + ) + # bindings for keyboard to command + self._create_key_bindings() + # command buffers + self._base_command = np.zeros(3) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __del__(self): + """Release the keyboard interface.""" + self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub) + self._keyboard_sub = None + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Keyboard Controller for SE(2): {self.__class__.__name__}\n" + msg += f"\tKeyboard name: {self._input.get_keyboard_name(self._keyboard)}\n" + msg += "\t----------------------------------------------\n" + msg += "\tReset all commands: L\n" + msg += "\tMove forward (along x-axis): Numpad 8 / Arrow Up\n" + msg += "\tMove backward (along x-axis): Numpad 2 / Arrow Down\n" + msg += "\tMove right (along y-axis): Numpad 4 / Arrow Right\n" + msg += "\tMove left (along y-axis): Numpad 6 / Arrow Left\n" + msg += "\tYaw positively (along z-axis): Numpad 7 / Z\n" + msg += "\tYaw negatively (along z-axis): Numpad 9 / X" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._base_command.fill(0.0) + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard. + + A list of available keys are present in the + `carb documentation `__. + + Args: + key: The keyboard button to check against. + func: The function to call when key is pressed. The callback function should not + take any arguments. + """ + self._additional_callbacks[key] = func + + def advance(self) -> np.ndarray: + """Provides the result from keyboard event state. + + Returns: + 3D array containing the linear (x,y) and angular velocity (z). + """ + return self._base_command + + """ + Internal helpers. + """ + + def _on_keyboard_event(self, event, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/dev-guide/latest/programmer_ref/input-devices/keyboard.html + """ + # apply the command when pressed + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name == "L": + self.reset() + elif event.input.name in self._INPUT_KEY_MAPPING: + self._base_command += self._INPUT_KEY_MAPPING[event.input.name] + # remove the command when un-pressed + if event.type == carb.input.KeyboardEventType.KEY_RELEASE: + if event.input.name in self._INPUT_KEY_MAPPING: + self._base_command -= self._INPUT_KEY_MAPPING[event.input.name] + # additional callbacks + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name in self._additional_callbacks: + self._additional_callbacks[event.input.name]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + self._INPUT_KEY_MAPPING = { + # forward command + "NUMPAD_8": np.asarray([1.0, 0.0, 0.0]) * self.v_x_sensitivity, + "UP": np.asarray([1.0, 0.0, 0.0]) * self.v_x_sensitivity, + # back command + "NUMPAD_2": np.asarray([-1.0, 0.0, 0.0]) * self.v_x_sensitivity, + "DOWN": np.asarray([-1.0, 0.0, 0.0]) * self.v_x_sensitivity, + # right command + "NUMPAD_4": np.asarray([0.0, 1.0, 0.0]) * self.v_y_sensitivity, + "LEFT": np.asarray([0.0, 1.0, 0.0]) * self.v_y_sensitivity, + # left command + "NUMPAD_6": np.asarray([0.0, -1.0, 0.0]) * self.v_y_sensitivity, + "RIGHT": np.asarray([0.0, -1.0, 0.0]) * self.v_y_sensitivity, + # yaw command (positive) + "NUMPAD_7": np.asarray([0.0, 0.0, 1.0]) * self.omega_z_sensitivity, + "Z": np.asarray([0.0, 0.0, 1.0]) * self.omega_z_sensitivity, + # yaw command (negative) + "NUMPAD_9": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, + "X": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, + } diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se3_keyboard.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se3_keyboard.py new file mode 100644 index 00000000000..8b4843624de --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/keyboard/se3_keyboard.py @@ -0,0 +1,193 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Keyboard controller for SE(3) control.""" + +import numpy as np +import weakref +from collections.abc import Callable +from scipy.spatial.transform import Rotation + +import carb +import omni + +from ..device_base import DeviceBase + + +class Se3Keyboard(DeviceBase): + """A keyboard controller for sending SE(3) commands as delta poses and binary command (open/close). + + This class is designed to provide a keyboard controller for a robotic arm with a gripper. + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Key bindings: + ============================== ================= ================= + Description Key (+ve axis) Key (-ve axis) + ============================== ================= ================= + Toggle gripper (open/close) K + Move along x-axis W S + Move along y-axis A D + Move along z-axis Q E + Rotate along x-axis Z X + Rotate along y-axis T G + Rotate along z-axis C V + ============================== ================= ================= + + .. seealso:: + + The official documentation for the keyboard interface: `Carb Keyboard Interface `__. + + """ + + def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + """Initialize the keyboard layer. + + Args: + pos_sensitivity: Magnitude of input position command scaling. Defaults to 0.05. + rot_sensitivity: Magnitude of scale input rotation commands scaling. Defaults to 0.5. + """ + # store inputs + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + # acquire omniverse interfaces + self._appwindow = omni.appwindow.get_default_app_window() + self._input = carb.input.acquire_input_interface() + self._keyboard = self._appwindow.get_keyboard() + # note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called. + self._keyboard_sub = self._input.subscribe_to_keyboard_events( + self._keyboard, + lambda event, *args, obj=weakref.proxy(self): obj._on_keyboard_event(event, *args), + ) + # bindings for keyboard to command + self._create_key_bindings() + # command buffers + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + # dictionary for additional callbacks + self._additional_callbacks = dict() + + def __del__(self): + """Release the keyboard interface.""" + self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub) + self._keyboard_sub = None + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Keyboard Controller for SE(3): {self.__class__.__name__}\n" + msg += f"\tKeyboard name: {self._input.get_keyboard_name(self._keyboard)}\n" + msg += "\t----------------------------------------------\n" + msg += "\tToggle gripper (open/close): K\n" + msg += "\tMove arm along x-axis: W/S\n" + msg += "\tMove arm along y-axis: A/D\n" + msg += "\tMove arm along z-axis: Q/E\n" + msg += "\tRotate arm along x-axis: Z/X\n" + msg += "\tRotate arm along y-axis: T/G\n" + msg += "\tRotate arm along z-axis: C/V" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard. + + A list of available keys are present in the + `carb documentation `__. + + Args: + key: The keyboard button to check against. + func: The function to call when key is pressed. The callback function should not + take any arguments. + """ + self._additional_callbacks[key] = func + + def advance(self) -> tuple[np.ndarray, bool]: + """Provides the result from keyboard event state. + + Returns: + A tuple containing the delta pose command and gripper commands. + """ + # convert to rotation vector + rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec() + # return the command and gripper state + return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper + + """ + Internal helpers. + """ + + def _on_keyboard_event(self, event, *args, **kwargs): + """Subscriber callback to when kit is updated. + + Reference: + https://docs.omniverse.nvidia.com/dev-guide/latest/programmer_ref/input-devices/keyboard.html + """ + # apply the command when pressed + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name == "L": + self.reset() + if event.input.name == "K": + self._close_gripper = not self._close_gripper + elif event.input.name in ["W", "S", "A", "D", "Q", "E"]: + self._delta_pos += self._INPUT_KEY_MAPPING[event.input.name] + elif event.input.name in ["Z", "X", "T", "G", "C", "V"]: + self._delta_rot += self._INPUT_KEY_MAPPING[event.input.name] + # remove the command when un-pressed + if event.type == carb.input.KeyboardEventType.KEY_RELEASE: + if event.input.name in ["W", "S", "A", "D", "Q", "E"]: + self._delta_pos -= self._INPUT_KEY_MAPPING[event.input.name] + elif event.input.name in ["Z", "X", "T", "G", "C", "V"]: + self._delta_rot -= self._INPUT_KEY_MAPPING[event.input.name] + # additional callbacks + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input.name in self._additional_callbacks: + self._additional_callbacks[event.input.name]() + + # since no error, we are fine :) + return True + + def _create_key_bindings(self): + """Creates default key binding.""" + self._INPUT_KEY_MAPPING = { + # toggle: gripper command + "K": True, + # x-axis (forward) + "W": np.asarray([1.0, 0.0, 0.0]) * self.pos_sensitivity, + "S": np.asarray([-1.0, 0.0, 0.0]) * self.pos_sensitivity, + # y-axis (left-right) + "A": np.asarray([0.0, 1.0, 0.0]) * self.pos_sensitivity, + "D": np.asarray([0.0, -1.0, 0.0]) * self.pos_sensitivity, + # z-axis (up-down) + "Q": np.asarray([0.0, 0.0, 1.0]) * self.pos_sensitivity, + "E": np.asarray([0.0, 0.0, -1.0]) * self.pos_sensitivity, + # roll (around x-axis) + "Z": np.asarray([1.0, 0.0, 0.0]) * self.rot_sensitivity, + "X": np.asarray([-1.0, 0.0, 0.0]) * self.rot_sensitivity, + # pitch (around y-axis) + "T": np.asarray([0.0, 1.0, 0.0]) * self.rot_sensitivity, + "G": np.asarray([0.0, -1.0, 0.0]) * self.rot_sensitivity, + # yaw (around z-axis) + "C": np.asarray([0.0, 0.0, 1.0]) * self.rot_sensitivity, + "V": np.asarray([0.0, 0.0, -1.0]) * self.rot_sensitivity, + } diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/__init__.py new file mode 100644 index 00000000000..ad22bdae4c5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Keyboard device for SE(2) and SE(3) control.""" + +from .openxr_device import OpenXRDevice +from .xr_cfg import XrCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/common.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/common.py new file mode 100644 index 00000000000..d21e564c96e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/common.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Standard set of hand joint names based on OpenXR specification. +# Input devices for dexterous hands can use this as a reference, +# but may provide any subset or superset of these joints. +HAND_JOINT_NAMES = [ + # Palm + "palm", + # Wrist + "wrist", + # Thumb + "thumb_metacarpal", + "thumb_proximal", + "thumb_distal", + "thumb_tip", + # Index + "index_metacarpal", + "index_proximal", + "index_intermediate", + "index_distal", + "index_tip", + # Middle + "middle_metacarpal", + "middle_proximal", + "middle_intermediate", + "middle_distal", + "middle_tip", + # Ring + "ring_metacarpal", + "ring_proximal", + "ring_intermediate", + "ring_distal", + "ring_tip", + # Little + "little_metacarpal", + "little_proximal", + "little_intermediate", + "little_distal", + "little_tip", +] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/openxr_device.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/openxr_device.py new file mode 100644 index 00000000000..8a2252d1f8d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/openxr_device.py @@ -0,0 +1,276 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OpenXR-powered device for teleoperation and interaction.""" + +import contextlib +import numpy as np +from collections.abc import Callable +from enum import Enum +from typing import Any + +import carb + +from isaaclab.devices.openxr.common import HAND_JOINT_NAMES +from isaaclab.devices.retargeter_base import RetargeterBase + +from ..device_base import DeviceBase +from .xr_cfg import XrCfg + +with contextlib.suppress(ModuleNotFoundError): + from isaacsim.xr.openxr import OpenXR, OpenXRSpec + from omni.kit.xr.core import XRCore + +from isaacsim.core.prims import SingleXFormPrim + + +class OpenXRDevice(DeviceBase): + """An OpenXR-powered device for teleoperation and interaction. + + This device tracks hand joints using OpenXR and makes them available as: + 1. A dictionary of joint poses (when used directly) + 2. Retargeted commands for robot control (when a retargeter is provided) + + Data format: + * Joint poses: Each pose is a 7D vector (x, y, z, qw, qx, qy, qz) in meters and quaternion units + * Dictionary keys: Joint names from HAND_JOINT_NAMES in isaaclab.devices.openxr.common + * Supported joints include palm, wrist, and joints for thumb, index, middle, ring and little fingers + + Teleop commands: + The device responds to several teleop commands that can be subscribed to via add_callback(): + * "START": Resume hand tracking data flow + * "STOP": Pause hand tracking data flow + * "RESET": Reset the tracking and signal simulation reset + + The device can track the left hand, right hand, head position, or any combination of these + based on the TrackingTarget enum values. When retargeters are provided, the raw tracking + data is transformed into robot control commands suitable for teleoperation. + """ + + class TrackingTarget(Enum): + """Enum class specifying what to track with OpenXR. + + Attributes: + HAND_LEFT: Track the left hand (index 0 in _get_raw_data output) + HAND_RIGHT: Track the right hand (index 1 in _get_raw_data output) + HEAD: Track the head/headset position (index 2 in _get_raw_data output) + """ + + HAND_LEFT = 0 + HAND_RIGHT = 1 + HEAD = 2 + + TELEOP_COMMAND_EVENT_TYPE = "teleop_command" + + def __init__( + self, + xr_cfg: XrCfg | None, + retargeters: list[RetargeterBase] | None = None, + ): + """Initialize the OpenXR device. + + Args: + xr_cfg: Configuration object for OpenXR settings. If None, default settings are used. + retargeters: List of retargeters to transform tracking data into robot commands. + If None or empty list, raw tracking data will be returned. + """ + super().__init__(retargeters) + self._openxr = OpenXR() + self._xr_cfg = xr_cfg or XrCfg() + self._additional_callbacks = dict() + self._vc_subscription = ( + XRCore.get_singleton() + .get_message_bus() + .create_subscription_to_pop_by_type( + carb.events.type_from_string(self.TELEOP_COMMAND_EVENT_TYPE), self._on_teleop_command + ) + ) + self._previous_joint_poses_left = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_right = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_headpose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + + # Specify the placement of the simulation when viewed in an XR device using a prim. + xr_anchor = SingleXFormPrim("/XRAnchor", position=self._xr_cfg.anchor_pos, orientation=self._xr_cfg.anchor_rot) + carb.settings.get_settings().set_float("/persistent/xr/profile/ar/render/nearPlane", self._xr_cfg.near_plane) + carb.settings.get_settings().set_string("/persistent/xr/profile/ar/anchorMode", "custom anchor") + carb.settings.get_settings().set_string("/xrstage/profile/ar/customAnchor", xr_anchor.prim_path) + + def __del__(self): + """Clean up resources when the object is destroyed. + + Properly unsubscribes from the XR message bus to prevent memory leaks + and resource issues when the device is no longer needed. + """ + if hasattr(self, "_vc_subscription") and self._vc_subscription is not None: + self._vc_subscription = None + + # No need to explicitly clean up OpenXR instance as it's managed by NVIDIA Isaac Sim + + def __str__(self) -> str: + """Returns a string containing information about the OpenXR hand tracking device. + + This provides details about the device configuration, tracking settings, + and available gesture commands. + + Returns: + Formatted string with device information + """ + + msg = f"OpenXR Hand Tracking Device: {self.__class__.__name__}\n" + msg += f"\tAnchor Position: {self._xr_cfg.anchor_pos}\n" + msg += f"\tAnchor Rotation: {self._xr_cfg.anchor_rot}\n" + + # Add retargeter information + if self._retargeters: + msg += "\tRetargeters:\n" + for i, retargeter in enumerate(self._retargeters): + msg += f"\t\t{i + 1}. {retargeter.__class__.__name__}\n" + else: + msg += "\tRetargeters: None (raw joint data output)\n" + + # Add available gesture commands + msg += "\t----------------------------------------------\n" + msg += "\tAvailable Gesture Commands:\n" + + # Check which callbacks are registered + start_avail = "START" in self._additional_callbacks + stop_avail = "STOP" in self._additional_callbacks + reset_avail = "RESET" in self._additional_callbacks + + msg += f"\t\tStart Teleoperation: {'✓' if start_avail else '✗'}\n" + msg += f"\t\tStop Teleoperation: {'✓' if stop_avail else '✗'}\n" + msg += f"\t\tReset Environment: {'✓' if reset_avail else '✗'}\n" + + # Add joint tracking information + msg += "\t----------------------------------------------\n" + msg += "\tTracked Joints: All 26 XR hand joints including:\n" + msg += "\t\t- Wrist, palm\n" + msg += "\t\t- Thumb (tip, intermediate joints)\n" + msg += "\t\t- Fingers (tip, distal, intermediate, proximal)\n" + + return msg + + """ + Operations + """ + + def reset(self): + self._previous_joint_poses_left = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_joint_poses_right = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + self._previous_headpose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind to client messages. + + Args: + key: The message type to bind to. Valid values are "START", "STOP", and "RESET". + func: The function to call when the message is received. The callback function should not + take any arguments. + """ + self._additional_callbacks[key] = func + + def _get_raw_data(self) -> Any: + """Get the latest tracking data from the OpenXR runtime. + + Returns: + Dictionary containing tracking data for: + - Left hand joint poses (26 joints with position and orientation) + - Right hand joint poses (26 joints with position and orientation) + - Head pose (position and orientation) + + Each pose is represented as a 7-element array: [x, y, z, qw, qx, qy, qz] + where the first 3 elements are position and the last 4 are quaternion orientation. + """ + return { + self.TrackingTarget.HAND_LEFT: self._calculate_joint_poses( + self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_LEFT_EXT), + self._previous_joint_poses_left, + ), + self.TrackingTarget.HAND_RIGHT: self._calculate_joint_poses( + self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT), + self._previous_joint_poses_right, + ), + self.TrackingTarget.HEAD: self._calculate_headpose(), + } + + """ + Internal helpers. + """ + + def _calculate_joint_poses(self, hand_joints, previous_joint_poses) -> dict[str, np.ndarray]: + if hand_joints is None: + return self._joints_to_dict(previous_joint_poses) + + hand_joints = np.array(hand_joints) + positions = np.array([[j.pose.position.x, j.pose.position.y, j.pose.position.z] for j in hand_joints]) + orientations = np.array([ + [j.pose.orientation.w, j.pose.orientation.x, j.pose.orientation.y, j.pose.orientation.z] + for j in hand_joints + ]) + location_flags = np.array([j.locationFlags for j in hand_joints]) + + pos_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0 + ori_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0 + + previous_joint_poses[pos_mask, 0:3] = positions[pos_mask] + previous_joint_poses[ori_mask, 3:7] = orientations[ori_mask] + + return self._joints_to_dict(previous_joint_poses) + + def _calculate_headpose(self) -> np.ndarray: + """Calculate the head pose from OpenXR. + + Returns: + numpy.ndarray: 7-element array containing head position (xyz) and orientation (wxyz) + """ + head_device = XRCore.get_singleton().get_input_device("displayDevice") + if head_device: + hmd = head_device.get_virtual_world_pose("") + position = hmd.ExtractTranslation() + quat = hmd.ExtractRotationQuat() + quati = quat.GetImaginary() + quatw = quat.GetReal() + + # Store in w, x, y, z order to match our convention + self._previous_headpose = np.array([ + position[0], + position[1], + position[2], + quatw, + quati[0], + quati[1], + quati[2], + ]) + + return self._previous_headpose + + def _joints_to_dict(self, joint_data: np.ndarray) -> dict[str, np.ndarray]: + """Convert joint array to dictionary using standard joint names. + + Args: + joint_data: Array of joint data (Nx6 for N joints) + + Returns: + Dictionary mapping joint names to their data + """ + return {joint_name: joint_data[i] for i, joint_name in enumerate(HAND_JOINT_NAMES)} + + def _on_teleop_command(self, event: carb.events.IEvent): + msg = event.payload["message"] + + if "start" in msg: + if "START" in self._additional_callbacks: + self._additional_callbacks["START"]() + elif "stop" in msg: + if "STOP" in self._additional_callbacks: + self._additional_callbacks["STOP"]() + elif "reset" in msg: + if "RESET" in self._additional_callbacks: + self._additional_callbacks["RESET"]() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/__init__.py new file mode 100644 index 00000000000..d0972ec02ee --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""Retargeters for mapping input device data to robot commands.""" + +from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter +from .manipulator.gripper_retargeter import GripperRetargeter +from .manipulator.se3_abs_retargeter import Se3AbsRetargeter +from .manipulator.se3_rel_retargeter import Se3RelRetargeter diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py new file mode 100644 index 00000000000..82cb7d94d93 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from typing import Any + +from isaaclab.devices.retargeter_base import RetargeterBase + + +class DexRetargeter(RetargeterBase): + """Retargets OpenXR hand joint data to DEX robot joint commands. + + This class implements the RetargeterBase interface to convert hand tracking data + into a format suitable for controlling DEX robot hands. + """ + + def __init__(self): + """Initialize the DEX retargeter.""" + super().__init__() + # TODO: Add any initialization parameters and state variables needed + pass + + def retarget(self, joint_data: dict[str, np.ndarray]) -> Any: + """Convert OpenXR hand joint poses to DEX robot commands. + + Args: + joint_data: Dictionary mapping OpenXR joint names to their pose data. + Each pose is a numpy array of shape (7,) containing + [x, y, z, qx, qy, qz, qw] for absolute mode or + [x, y, z, roll, pitch, yaw] for relative mode. + + Returns: + Retargeted data in the format expected by DEX robot control interface. + TODO: Specify the exact return type and format + """ + # TODO: Implement the retargeting logic + raise NotImplementedError("DexRetargeter.retarget() not implemented") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml new file mode 100644 index 00000000000..a622be71c95 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml @@ -0,0 +1,25 @@ +retargeting: + finger_tip_link_names: + - GR1T2_fourier_hand_6dof_L_thumb_distal_link + - GR1T2_fourier_hand_6dof_L_index_intermediate_link + - GR1T2_fourier_hand_6dof_L_middle_intermediate_link + - GR1T2_fourier_hand_6dof_L_ring_intermediate_link + - GR1T2_fourier_hand_6dof_L_pinky_intermediate_link + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - L_index_proximal_joint + - L_middle_proximal_joint + - L_pinky_proximal_joint + - L_ring_proximal_joint + - L_index_intermediate_joint + - L_middle_intermediate_joint + - L_pinky_intermediate_joint + - L_ring_intermediate_joint + - L_thumb_proximal_yaw_joint + - L_thumb_proximal_pitch_joint + - L_thumb_distal_joint + - L_thumb_distal_joint + type: DexPilot + urdf_path: /tmp/GR1_T2_left_hand.urdf + wrist_link_name: l_hand_base_link diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml new file mode 100644 index 00000000000..b090f39e5fd --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml @@ -0,0 +1,24 @@ +retargeting: + finger_tip_link_names: + - GR1T2_fourier_hand_6dof_R_thumb_distal_link + - GR1T2_fourier_hand_6dof_R_index_intermediate_link + - GR1T2_fourier_hand_6dof_R_middle_intermediate_link + - GR1T2_fourier_hand_6dof_R_ring_intermediate_link + - GR1T2_fourier_hand_6dof_R_pinky_intermediate_link + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - R_index_proximal_joint + - R_middle_proximal_joint + - R_pinky_proximal_joint + - R_ring_proximal_joint + - R_index_intermediate_joint + - R_middle_intermediate_joint + - R_pinky_intermediate_joint + - R_ring_intermediate_joint + - R_thumb_proximal_yaw_joint + - R_thumb_proximal_pitch_joint + - R_thumb_distal_joint + type: DexPilot + urdf_path: /tmp/GR1_T2_right_hand.urdf + wrist_link_name: r_hand_base_link diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py new file mode 100644 index 00000000000..0750392b0ed --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py @@ -0,0 +1,260 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import os +import torch +import yaml +from scipy.spatial.transform import Rotation as R + +import omni.log +from dex_retargeting.retargeting_config import RetargetingConfig + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# The index to map the OpenXR hand joints to the hand joints used +# in Dex-retargeting. +_HAND_JOINTS_INDEX = [1, 2, 3, 4, 5, 7, 8, 9, 10, 12, 13, 14, 15, 17, 18, 19, 20, 22, 23, 24, 25] + +# The transformation matrices to convert hand pose to canonical view. +_OPERATOR2MANO_RIGHT = np.array([ + [0, -1, 0], + [-1, 0, 0], + [0, 0, -1], +]) + +_OPERATOR2MANO_LEFT = np.array([ + [0, -1, 0], + [-1, 0, 0], + [0, 0, -1], +]) + +_LEFT_HAND_JOINT_NAMES = [ + "L_index_proximal_joint", + "L_index_intermediate_joint", + "L_middle_proximal_joint", + "L_middle_intermediate_joint", + "L_pinky_proximal_joint", + "L_pinky_intermediate_joint", + "L_ring_proximal_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_yaw_joint", + "L_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", +] + + +_RIGHT_HAND_JOINT_NAMES = [ + "R_index_proximal_joint", + "R_index_intermediate_joint", + "R_middle_proximal_joint", + "R_middle_intermediate_joint", + "R_pinky_proximal_joint", + "R_pinky_intermediate_joint", + "R_ring_proximal_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_yaw_joint", + "R_thumb_proximal_pitch_joint", + "R_thumb_distal_joint", +] + + +class GR1TR2DexRetargeting: + """A class for hand retargeting with GR1Fourier. + + Handles retargeting of OpenXRhand tracking data to GR1T2 robot hand joint angles. + """ + + def __init__( + self, + hand_joint_names: list[str], + right_hand_config_filename: str = "fourier_hand_right_dexpilot.yml", + left_hand_config_filename: str = "fourier_hand_left_dexpilot.yml", + left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_left_hand.urdf", + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_right_hand.urdf", + ): + """Initialize the hand retargeting. + + Args: + hand_joint_names: Names of hand joints in the robot model + right_hand_config_filename: Config file for right hand retargeting + left_hand_config_filename: Config file for left hand retargeting + """ + data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "data/")) + config_dir = os.path.join(data_dir, "configs/dex-retargeting") + + # Download urdf files from aws + local_left_urdf_path = retrieve_file_path(left_hand_urdf_path, force_download=True) + local_right_urdf_path = retrieve_file_path(right_hand_urdf_path, force_download=True) + + left_config_path = os.path.join(config_dir, left_hand_config_filename) + right_config_path = os.path.join(config_dir, right_hand_config_filename) + + # Update the YAML files with the correct URDF paths + self._update_yaml_with_urdf_path(left_config_path, local_left_urdf_path) + self._update_yaml_with_urdf_path(right_config_path, local_right_urdf_path) + + self._dex_left_hand = RetargetingConfig.load_from_file(left_config_path).build() + self._dex_right_hand = RetargetingConfig.load_from_file(right_config_path).build() + + self.left_dof_names = self._dex_left_hand.optimizer.robot.dof_joint_names + self.right_dof_names = self._dex_right_hand.optimizer.robot.dof_joint_names + self.dof_names = self.left_dof_names + self.right_dof_names + self.isaac_lab_hand_joint_names = hand_joint_names + + omni.log.info("[GR1T2DexRetargeter] init done.") + + def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): + """Update YAML file with the correct URDF path. + + Args: + yaml_path: Path to the YAML configuration file + urdf_path: Path to the URDF file to use + """ + try: + # Read the YAML file + with open(yaml_path) as file: + config = yaml.safe_load(file) + + # Update the URDF path in the configuration + if "retargeting" in config: + config["retargeting"]["urdf_path"] = urdf_path + omni.log.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + omni.log.warn(f"Unable to find 'retargeting' section in {yaml_path}") + + # Write the updated configuration back to the file + with open(yaml_path, "w") as file: + yaml.dump(config, file) + + except Exception as e: + omni.log.error(f"Error updating YAML file {yaml_path}: {e}") + + def convert_hand_joints(self, hand_poses: dict[str, np.ndarray], operator2mano: np.ndarray) -> np.ndarray: + """Prepares the hand joints data for retargeting. + + Args: + hand_poses: Dictionary containing hand pose data with joint positions and rotations + operator2mano: Transformation matrix to convert from operator to MANO frame + + Returns: + Joint positions with shape (21, 3) + """ + joint_position = np.zeros((21, 3)) + hand_joints = list(hand_poses.values()) + for i in range(len(_HAND_JOINTS_INDEX)): + joint = hand_joints[_HAND_JOINTS_INDEX[i]] + joint_position[i] = joint[:3] + + # Convert hand pose to the canonical frame. + joint_position = joint_position - joint_position[0:1, :] + xr_wrist_quat = hand_poses.get("wrist")[3:] + # OpenXR hand uses w,x,y,z order for quaternions but scipy uses x,y,z,w order + wrist_rot = R.from_quat([xr_wrist_quat[1], xr_wrist_quat[2], xr_wrist_quat[3], xr_wrist_quat[0]]).as_matrix() + + return joint_position @ wrist_rot @ operator2mano + + def compute_ref_value(self, joint_position: np.ndarray, indices: np.ndarray, retargeting_type: str) -> np.ndarray: + """Computes reference value for retargeting. + + Args: + joint_position: Joint positions array + indices: Target link indices + retargeting_type: Type of retargeting ("POSITION" or other) + + Returns: + Reference value in cartesian space + """ + if retargeting_type == "POSITION": + return joint_position[indices, :] + else: + origin_indices = indices[0, :] + task_indices = indices[1, :] + ref_value = joint_position[task_indices, :] - joint_position[origin_indices, :] + return ref_value + + def compute_one_hand( + self, hand_joints: dict[str, np.ndarray], retargeting: RetargetingConfig, operator2mano: np.ndarray + ) -> np.ndarray: + """Computes retargeted joint angles for one hand. + + Args: + hand_joints: Dictionary containing hand joint data + retargeting: Retargeting configuration object + operator2mano: Transformation matrix from operator to MANO frame + + Returns: + Retargeted joint angles + """ + joint_pos = self.convert_hand_joints(hand_joints, operator2mano) + ref_value = self.compute_ref_value( + joint_pos, + indices=retargeting.optimizer.target_link_human_indices, + retargeting_type=retargeting.optimizer.retargeting_type, + ) + # Enable gradient calculation and inference mode in case some other script has disabled it + # This is necessary for the retargeting to work since it uses gradient features that + # are not available in inference mode + with torch.enable_grad(): + with torch.inference_mode(False): + return retargeting.retarget(ref_value) + + def get_joint_names(self) -> list[str]: + """Returns list of all joint names.""" + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + """Returns list of left hand joint names.""" + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + """Returns list of right hand joint names.""" + return self.right_dof_names + + def get_hand_indices(self, robot) -> np.ndarray: + """Gets indices of hand joints in robot's DOF array. + + Args: + robot: Robot object containing DOF information + + Returns: + Array of joint indices + """ + return np.array([robot.dof_names.index(name) for name in self.dof_names], dtype=np.int64) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for left hand. + + Args: + left_hand_poses: Dictionary of left hand joint poses + + Returns: + Retargeted joint angles for left hand + """ + if left_hand_poses is not None: + left_hand_q = self.compute_one_hand(left_hand_poses, self._dex_left_hand, _OPERATOR2MANO_LEFT) + else: + left_hand_q = np.zeros(len(_LEFT_HAND_JOINT_NAMES)) + return left_hand_q + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for right hand. + + Args: + right_hand_poses: Dictionary of right hand joint poses + + Returns: + Retargeted joint angles for right hand + """ + if right_hand_poses is not None: + right_hand_q = self.compute_one_hand(right_hand_poses, self._dex_right_hand, _OPERATOR2MANO_RIGHT) + else: + right_hand_q = np.zeros(len(_RIGHT_HAND_JOINT_NAMES)) + return right_hand_q diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py new file mode 100644 index 00000000000..fe2a8563eab --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import contextlib +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +# This import exception is suppressed because gr1_t2_dex_retargeting_utils depends on pinocchio which is not available on windows +with contextlib.suppress(Exception): + from .gr1_t2_dex_retargeting_utils import GR1TR2DexRetargeting + + +class GR1T2Retargeter(RetargeterBase): + """Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands. + + This retargeter maps hand tracking data from OpenXR to joint commands for the GR1T2 robot's hands. + It handles both left and right hands, converting poses of the hands in OpenXR format joint angles for the GR1T2 robot's hands. + """ + + def __init__( + self, + enable_visualization: bool = False, + num_open_xr_hand_joints: int = 100, + device: torch.device = torch.device("cuda:0"), + hand_joint_names: list[str] = [], + ): + """Initialize the GR1T2 hand retargeter. + + Args: + enable_visualization: If True, visualize tracked hand joints + num_open_xr_hand_joints: Number of joints tracked by OpenXR + device: PyTorch device for computations + hand_joint_names: List of robot hand joint names + """ + + self._hand_joint_names = hand_joint_names + self._hands_controller = GR1TR2DexRetargeting(self._hand_joint_names) + + # Initialize visualization if enabled + self._enable_visualization = enable_visualization + self._num_open_xr_hand_joints = num_open_xr_hand_joints + self._device = device + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Convert hand joint poses to robot end-effector commands. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + + Returns: + tuple containing: + Left wrist pose + Right wrist pose in USD frame + Retargeted hand joint angles + """ + + # Access the left and right hand data using the enum key + left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] + right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) + + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + + self._markers.visualize(translations=torch.tensor(joints_position, device=self._device)) + + # Create array of zeros with length matching number of joint names + left_hands_pos = self._hands_controller.compute_left(left_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + left_retargeted_hand_joints[indexes] = left_hands_pos + left_hand_joints = left_retargeted_hand_joints + + right_hands_pos = self._hands_controller.compute_right(right_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + right_retargeted_hand_joints[indexes] = right_hands_pos + right_hand_joints = right_retargeted_hand_joints + retargeted_hand_joints = left_hand_joints + right_hand_joints + + return left_wrist, self._retarget_abs(right_wrist), retargeted_hand_joints + + def _retarget_abs(self, wrist: np.ndarray) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + wrist: Wrist pose data from OpenXR + + Returns: + Retargeted wrist pose in USD control frame + """ + + # Convert wrist data in openxr frame to usd control frame + + # Create pose object for openxr_right_wrist_in_world + # Note: The pose utils require torch tensors + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + openxr_right_wrist_in_world = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + + # The usd control frame is 180 degrees rotated around z axis wrt to the openxr frame + # This was determined through trial and error + zero_pos = torch.zeros(3, dtype=torch.float32) + # 180 degree rotation around z axis + z_axis_rot_quat = torch.tensor([0, 0, 0, 1], dtype=torch.float32) + usd_right_roll_link_in_openxr_right_wrist = PoseUtils.make_pose( + zero_pos, PoseUtils.matrix_from_quat(z_axis_rot_quat) + ) + + # Convert wrist pose in openxr frame to usd control frame + usd_right_roll_link_in_world = PoseUtils.pose_in_A_to_pose_in_B( + usd_right_roll_link_in_openxr_right_wrist, openxr_right_wrist_in_world + ) + + # extract position and rotation + usd_right_roll_link_in_world_pos, usd_right_roll_link_in_world_mat = PoseUtils.unmake_pose( + usd_right_roll_link_in_world + ) + usd_right_roll_link_in_world_quat = PoseUtils.quat_from_matrix(usd_right_roll_link_in_world_mat) + + return np.concatenate([usd_right_roll_link_in_world_pos, usd_right_roll_link_in_world_quat]) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/__init__.py new file mode 100644 index 00000000000..cd5ba96787a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Franka manipulator retargeting module. + +This module provides functionality for retargeting motion to Franka robots. +""" + +from .gripper_retargeter import GripperRetargeter +from .se3_abs_retargeter import Se3AbsRetargeter +from .se3_rel_retargeter import Se3RelRetargeter diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py new file mode 100644 index 00000000000..db176c67e55 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py @@ -0,0 +1,86 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from typing import Final + +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase + + +class GripperRetargeter(RetargeterBase): + """Retargeter specifically for gripper control based on hand tracking data. + + This retargeter analyzes the distance between thumb and index finger tips to determine + whether the gripper should be open or closed. It includes hysteresis to prevent rapid + toggling between states when the finger distance is near the threshold. + + Features: + - Tracks thumb and index finger distance + - Implements hysteresis for stable gripper control + - Outputs boolean command (True = close gripper, False = open gripper) + """ + + GRIPPER_CLOSE_METERS: Final[float] = 0.03 + GRIPPER_OPEN_METERS: Final[float] = 0.05 + + def __init__( + self, + bound_hand: OpenXRDevice.TrackingTarget, + ): + """Initialize the gripper retargeter.""" + # Store the hand to track + if bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" + " OpenXRDevice.TrackingTarget.HAND_RIGHT" + ) + self.bound_hand = bound_hand + # Initialize gripper state + self._previous_gripper_command = False + + def retarget(self, data: dict) -> bool: + """Convert hand joint poses to gripper command. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + The joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES + + Returns: + bool: Gripper command where True = close gripper, False = open gripper + """ + # Extract key joint poses + hand_data = data[self.bound_hand] + thumb_tip = hand_data["thumb_tip"] + index_tip = hand_data["index_tip"] + + # Calculate gripper command with hysteresis + gripper_command = self._calculate_gripper_command(thumb_tip[:3], index_tip[:3]) + return gripper_command + + def _calculate_gripper_command(self, thumb_pos: np.ndarray, index_pos: np.ndarray) -> bool: + """Calculate gripper command from finger positions with hysteresis. + + Args: + thumb_pos: 3D position of thumb tip + index_pos: 3D position of index tip + + Returns: + bool: Gripper command (True = close, False = open) + """ + distance = np.linalg.norm(thumb_pos - index_pos) + + # Apply hysteresis to prevent rapid switching + if distance > self.GRIPPER_OPEN_METERS: + self._previous_gripper_command = False + elif distance < self.GRIPPER_CLOSE_METERS: + self._previous_gripper_command = True + + return self._previous_gripper_command diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py new file mode 100644 index 00000000000..821ce1f9351 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py @@ -0,0 +1,158 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG + + +class Se3AbsRetargeter(RetargeterBase): + """Retargets OpenXR hand tracking data to end-effector commands using absolute positioning. + + This retargeter maps hand joint poses directly to robot end-effector positions and orientations, + rather than using relative movements. It can either: + - Use the wrist position and orientation + - Use the midpoint between thumb and index finger (pinch position) + + Features: + - Optional constraint to zero out X/Y rotations (keeping only Z-axis rotation) + - Optional visualization of the target end-effector pose + """ + + def __init__( + self, + bound_hand: OpenXRDevice.TrackingTarget, + zero_out_xy_rotation: bool = False, + use_wrist_rotation: bool = False, + use_wrist_position: bool = False, + enable_visualization: bool = False, + ): + """Initialize the retargeter. + + Args: + bound_hand: The hand to track (OpenXRDevice.TrackingTarget.HAND_LEFT or OpenXRDevice.TrackingTarget.HAND_RIGHT) + zero_out_xy_rotation: If True, zero out rotation around x and y axes + use_wrist_rotation: If True, use wrist rotation instead of finger average + use_wrist_position: If True, use wrist position instead of pinch position + enable_visualization: If True, visualize the target pose in the scene + """ + if bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" + " OpenXRDevice.TrackingTarget.HAND_RIGHT" + ) + self.bound_hand = bound_hand + + self._zero_out_xy_rotation = zero_out_xy_rotation + self._use_wrist_rotation = use_wrist_rotation + self._use_wrist_position = use_wrist_position + + # Initialize visualization if enabled + self._enable_visualization = enable_visualization + if enable_visualization: + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + self._goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + self._goal_marker.set_visibility(True) + self._visualization_pos = np.zeros(3) + self._visualization_rot = np.array([1.0, 0.0, 0.0, 0.0]) + + def retarget(self, data: dict) -> np.ndarray: + """Convert hand joint poses to robot end-effector command. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + The joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES + + Returns: + np.ndarray: 7D array containing position (xyz) and orientation (quaternion) + for the robot end-effector + """ + # Extract key joint poses from the bound hand + hand_data = data[self.bound_hand] + thumb_tip = hand_data.get("thumb_tip") + index_tip = hand_data.get("index_tip") + wrist = hand_data.get("wrist") + + ee_command = self._retarget_abs(thumb_tip, index_tip, wrist) + + return ee_command + + def _retarget_abs(self, thumb_tip: np.ndarray, index_tip: np.ndarray, wrist: np.ndarray) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + thumb_tip: 7D array containing position (xyz) and orientation (quaternion) + for the thumb tip + index_tip: 7D array containing position (xyz) and orientation (quaternion) + for the index tip + wrist: 7D array containing position (xyz) and orientation (quaternion) + for the wrist + + Returns: + np.ndarray: 7D array containing position (xyz) and orientation (quaternion) + for the robot end-effector + """ + + # Get position + if self._use_wrist_position: + position = wrist[:3] + else: + position = (thumb_tip[:3] + index_tip[:3]) / 2 + + # Get rotation + if self._use_wrist_rotation: + # wrist is w,x,y,z but scipy expects x,y,z,w + base_rot = Rotation.from_quat([*wrist[4:], wrist[3]]) + else: + # Average the orientations of thumb and index using SLERP + # thumb_tip is w,x,y,z but scipy expects x,y,z,w + r0 = Rotation.from_quat([*thumb_tip[4:], thumb_tip[3]]) + # index_tip is w,x,y,z but scipy expects x,y,z,w + r1 = Rotation.from_quat([*index_tip[4:], index_tip[3]]) + key_times = [0, 1] + slerp = Slerp(key_times, Rotation.concatenate([r0, r1])) + base_rot = slerp([0.5])[0] + + # Apply additional x-axis rotation to align with pinch gesture + final_rot = base_rot * Rotation.from_euler("x", 90, degrees=True) + + if self._zero_out_xy_rotation: + z, y, x = final_rot.as_euler("ZYX") + y = 0.0 # Zero out rotation around y-axis + x = 0.0 # Zero out rotation around x-axis + final_rot = Rotation.from_euler("ZYX", [z, y, x]) * Rotation.from_euler("X", np.pi, degrees=False) + + # Convert back to w,x,y,z format + quat = final_rot.as_quat() + rotation = np.array([quat[3], quat[0], quat[1], quat[2]]) # Output remains w,x,y,z + + # Update visualization if enabled + if self._enable_visualization: + self._visualization_pos = position + self._visualization_rot = rotation + self._update_visualization() + + return np.concatenate([position, rotation]) + + def _update_visualization(self): + """Update visualization markers with current pose. + + If visualization is enabled, the target end-effector pose is visualized in the scene. + """ + if self._enable_visualization: + trans = np.array([self._visualization_pos]) + quat = Rotation.from_matrix(self._visualization_rot).as_quat() + rot = np.array([np.array([quat[3], quat[0], quat[1], quat[2]])]) + self._goal_marker.visualize(translations=trans, orientations=rot) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py new file mode 100644 index 00000000000..06e44495cbc --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py @@ -0,0 +1,200 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG + + +class Se3RelRetargeter(RetargeterBase): + """Retargets OpenXR hand tracking data to end-effector commands using relative positioning. + + This retargeter calculates delta poses between consecutive hand joint poses to generate incremental robot movements. + It can either: + - Use the wrist position and orientation + - Use the midpoint between thumb and index finger (pinch position) + + Features: + - Optional constraint to zero out X/Y rotations (keeping only Z-axis rotation) + - Motion smoothing with adjustable parameters + - Optional visualization of the target end-effector pose + """ + + def __init__( + self, + bound_hand: OpenXRDevice.TrackingTarget, + zero_out_xy_rotation: bool = False, + use_wrist_rotation: bool = False, + use_wrist_position: bool = True, + delta_pos_scale_factor: float = 10.0, + delta_rot_scale_factor: float = 10.0, + alpha_pos: float = 0.5, + alpha_rot: float = 0.5, + enable_visualization: bool = False, + ): + """Initialize the relative motion retargeter. + + Args: + bound_hand: The hand to track (OpenXRDevice.TrackingTarget.HAND_LEFT or OpenXRDevice.TrackingTarget.HAND_RIGHT) + zero_out_xy_rotation: If True, ignore rotations around x and y axes, allowing only z-axis rotation + use_wrist_rotation: If True, use wrist rotation for control instead of averaging finger orientations + use_wrist_position: If True, use wrist position instead of pinch position (midpoint between fingers) + delta_pos_scale_factor: Amplification factor for position changes (higher = larger robot movements) + delta_rot_scale_factor: Amplification factor for rotation changes (higher = larger robot rotations) + alpha_pos: Position smoothing parameter (0-1); higher values track more closely to input, lower values smooth more + alpha_rot: Rotation smoothing parameter (0-1); higher values track more closely to input, lower values smooth more + enable_visualization: If True, show a visual marker representing the target end-effector pose + """ + # Store the hand to track + if bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + raise ValueError( + "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" + " OpenXRDevice.TrackingTarget.HAND_RIGHT" + ) + self.bound_hand = bound_hand + + self._zero_out_xy_rotation = zero_out_xy_rotation + self._use_wrist_rotation = use_wrist_rotation + self._use_wrist_position = use_wrist_position + self._delta_pos_scale_factor = delta_pos_scale_factor + self._delta_rot_scale_factor = delta_rot_scale_factor + self._alpha_pos = alpha_pos + self._alpha_rot = alpha_rot + + # Initialize smoothing state + self._smoothed_delta_pos = np.zeros(3) + self._smoothed_delta_rot = np.zeros(3) + + # Define thresholds for small movements + self._position_threshold = 0.001 + self._rotation_threshold = 0.01 + + # Initialize visualization if enabled + self._enable_visualization = enable_visualization + if enable_visualization: + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + self._goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + self._goal_marker.set_visibility(True) + self._visualization_pos = np.zeros(3) + self._visualization_rot = np.array([1.0, 0.0, 0.0, 0.0]) + + self._previous_thumb_tip = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + self._previous_index_tip = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + self._previous_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + def retarget(self, data: dict) -> np.ndarray: + """Convert hand joint poses to robot end-effector command. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + The joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES + + Returns: + np.ndarray: 6D array containing position (xyz) and rotation vector (rx,ry,rz) + for the robot end-effector + """ + # Extract key joint poses from the bound hand + hand_data = data[self.bound_hand] + thumb_tip = hand_data.get("thumb_tip") + index_tip = hand_data.get("index_tip") + wrist = hand_data.get("wrist") + + delta_thumb_tip = self._calculate_delta_pose(thumb_tip, self._previous_thumb_tip) + delta_index_tip = self._calculate_delta_pose(index_tip, self._previous_index_tip) + delta_wrist = self._calculate_delta_pose(wrist, self._previous_wrist) + ee_command = self._retarget_rel(delta_thumb_tip, delta_index_tip, delta_wrist) + + self._previous_thumb_tip = thumb_tip.copy() + self._previous_index_tip = index_tip.copy() + self._previous_wrist = wrist.copy() + + return ee_command + + def _calculate_delta_pose(self, joint_pose: np.ndarray, previous_joint_pose: np.ndarray) -> np.ndarray: + """Calculate delta pose from previous joint pose. + + Args: + joint_pose: Current joint pose (position and orientation) + previous_joint_pose: Previous joint pose for the same joint + + Returns: + np.ndarray: 6D array with position delta (xyz) and rotation delta as axis-angle (rx,ry,rz) + """ + delta_pos = joint_pose[:3] - previous_joint_pose[:3] + abs_rotation = Rotation.from_quat([*joint_pose[4:7], joint_pose[3]]) + previous_rot = Rotation.from_quat([*previous_joint_pose[4:7], previous_joint_pose[3]]) + relative_rotation = abs_rotation * previous_rot.inv() + return np.concatenate([delta_pos, relative_rotation.as_rotvec()]) + + def _retarget_rel(self, thumb_tip: np.ndarray, index_tip: np.ndarray, wrist: np.ndarray) -> np.ndarray: + """Handle relative (delta) pose retargeting. + + Args: + thumb_tip: Delta pose of thumb tip + index_tip: Delta pose of index tip + wrist: Delta pose of wrist + + Returns: + np.ndarray: 6D array with position delta (xyz) and rotation delta (rx,ry,rz) + """ + # Get position + if self._use_wrist_position: + position = wrist[:3] + else: + position = (thumb_tip[:3] + index_tip[:3]) / 2 + + # Get rotation + if self._use_wrist_rotation: + rotation = wrist[3:6] # rx, ry, rz + else: + rotation = (thumb_tip[3:6] + index_tip[3:6]) / 2 + + # Apply zero_out_xy_rotation regardless of rotation source + if self._zero_out_xy_rotation: + rotation[0] = 0 # x-axis + rotation[1] = 0 # y-axis + + # Smooth and scale position + self._smoothed_delta_pos = self._alpha_pos * position + (1 - self._alpha_pos) * self._smoothed_delta_pos + if np.linalg.norm(self._smoothed_delta_pos) < self._position_threshold: + self._smoothed_delta_pos = np.zeros(3) + position = self._smoothed_delta_pos * self._delta_pos_scale_factor + + # Smooth and scale rotation + self._smoothed_delta_rot = self._alpha_rot * rotation + (1 - self._alpha_rot) * self._smoothed_delta_rot + if np.linalg.norm(self._smoothed_delta_rot) < self._rotation_threshold: + self._smoothed_delta_rot = np.zeros(3) + rotation = self._smoothed_delta_rot * self._delta_rot_scale_factor + + # Update visualization if enabled + if self._enable_visualization: + # Convert rotation vector to quaternion and combine with current rotation + delta_quat = Rotation.from_rotvec(rotation).as_quat() # x, y, z, w format + current_rot = Rotation.from_quat([self._visualization_rot[1:], self._visualization_rot[0]]) + new_rot = Rotation.from_quat(delta_quat) * current_rot + self._visualization_pos = self._visualization_pos + position + # Convert back to w, x, y, z format + self._visualization_rot = np.array([new_rot.as_quat()[3], *new_rot.as_quat()[:3]]) + self._update_visualization() + + return np.concatenate([position, rotation]) + + def _update_visualization(self): + """Update visualization markers with current pose.""" + if self._enable_visualization: + trans = np.array([self._visualization_pos]) + quat = Rotation.from_matrix(self._visualization_rot).as_quat() + rot = np.array([np.array([quat[3], quat[0], quat[1], quat[2]])]) + self._goal_marker.visualize(translations=trans, orientations=rot) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/xr_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/xr_cfg.py new file mode 100644 index 00000000000..b3b05fdcfa8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/openxr/xr_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +from __future__ import annotations + +from isaaclab.utils import configclass + + +@configclass +class XrCfg: + """Configuration for viewing and interacting with the environment through an XR device.""" + + anchor_pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Specifies the position (in m) of the simulation when viewed in an XR device. + + Specifically: this position will appear at the origin of the XR device's local coordinate frame. + """ + + anchor_rot: tuple[float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Specifies the rotation (as a quaternion) of the simulation when viewed in an XR device. + + Specifically: this rotation will determine how the simulation is rotated with respect to the + origin of the XR device's local coordinate frame. + + This quantity is only effective if :attr:`xr_anchor_pos` is set. + """ + + near_plane: float = 0.15 + """Specifies the near plane distance for the XR device. + + This value determines the closest distance at which objects will be rendered in the XR device. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/retargeter_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/retargeter_base.py new file mode 100644 index 00000000000..185dd0747df --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/retargeter_base.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from abc import ABC, abstractmethod +from typing import Any + + +class RetargeterBase(ABC): + """Base interface for input data retargeting. + + This abstract class defines the interface for components that transform + raw device data into robot control commands. Implementations can handle + various types of transformations including: + - Hand joint data to end-effector poses + - Input device commands to robot movements + - Sensor data to control signals + """ + + @abstractmethod + def retarget(self, data: Any) -> Any: + """Retarget input data to desired output format. + + Args: + data: Raw input data to be transformed + + Returns: + Retargeted data in implementation-specific format + """ + pass diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/__init__.py new file mode 100644 index 00000000000..5b1c49dc882 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spacemouse device for SE(2) and SE(3) control.""" + +from .se2_spacemouse import Se2SpaceMouse +from .se3_spacemouse import Se3SpaceMouse diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se2_spacemouse.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se2_spacemouse.py new file mode 100644 index 00000000000..49ec2e224cd --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se2_spacemouse.py @@ -0,0 +1,159 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spacemouse controller for SE(2) control.""" + +import hid +import numpy as np +import threading +import time +from collections.abc import Callable + +from ..device_base import DeviceBase +from .utils import convert_buffer + + +class Se2SpaceMouse(DeviceBase): + r"""A space-mouse controller for sending SE(2) commands as delta poses. + + This class implements a space-mouse controller to provide commands to mobile base. + It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`. + + Note: + The interface finds and uses the first supported device connected to the computer. + + Currently tested for following devices: + + - SpaceMouse Compact: https://3dconnexion.com/de/product/spacemouse-compact/ + + .. _HID-API: https://github.com/libusb/hidapi + + """ + + def __init__(self, v_x_sensitivity: float = 0.8, v_y_sensitivity: float = 0.4, omega_z_sensitivity: float = 1.0): + """Initialize the spacemouse layer. + + Args: + v_x_sensitivity: Magnitude of linear velocity along x-direction scaling. Defaults to 0.8. + v_y_sensitivity: Magnitude of linear velocity along y-direction scaling. Defaults to 0.4. + omega_z_sensitivity: Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + """ + # store inputs + self.v_x_sensitivity = v_x_sensitivity + self.v_y_sensitivity = v_y_sensitivity + self.omega_z_sensitivity = omega_z_sensitivity + # acquire device interface + self._device = hid.device() + self._find_device() + # command buffers + self._base_command = np.zeros(3) + # dictionary for additional callbacks + self._additional_callbacks = dict() + # run a thread for listening to device updates + self._thread = threading.Thread(target=self._run_device) + self._thread.daemon = True + self._thread.start() + + def __del__(self): + """Destructor for the class.""" + self._thread.join() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Spacemouse Controller for SE(2): {self.__class__.__name__}\n" + msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n" + msg += f"\tProduct: {self._device.get_product_string()}\n" + msg += "\t----------------------------------------------\n" + msg += "\tRight button: reset command\n" + msg += "\tMove mouse laterally: move base horizontally in x-y plane\n" + msg += "\tTwist mouse about z-axis: yaw base about a corresponding axis" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._base_command.fill(0.0) + + def add_callback(self, key: str, func: Callable): + # check keys supported by callback + if key not in ["L", "R"]: + raise ValueError(f"Only left (L) and right (R) buttons supported. Provided: {key}.") + # TODO: Improve this to allow multiple buttons on same key. + self._additional_callbacks[key] = func + + def advance(self) -> np.ndarray: + """Provides the result from spacemouse event state. + + Returns: + A 3D array containing the linear (x,y) and angular velocity (z). + """ + return self._base_command + + """ + Internal helpers. + """ + + def _find_device(self): + """Find the device connected to computer.""" + found = False + # implement a timeout for device search + for _ in range(5): + for device in hid.enumerate(): + if device["product_string"] == "SpaceMouse Compact": + # set found flag + found = True + vendor_id = device["vendor_id"] + product_id = device["product_id"] + # connect to the device + self._device.open(vendor_id, product_id) + # check if device found + if not found: + time.sleep(1.0) + else: + break + # no device found: return false + if not found: + raise OSError("No device found by SpaceMouse. Is the device connected?") + + def _run_device(self): + """Listener thread that keeps pulling new messages.""" + # keep running + while True: + # read the device data + data = self._device.read(13) + if data is not None: + # readings from 6-DoF sensor + if data[0] == 1: + # along y-axis + self._base_command[1] = self.v_y_sensitivity * convert_buffer(data[1], data[2]) + # along x-axis + self._base_command[0] = self.v_x_sensitivity * convert_buffer(data[3], data[4]) + elif data[0] == 2: + # along z-axis + self._base_command[2] = self.omega_z_sensitivity * convert_buffer(data[3], data[4]) + # readings from the side buttons + elif data[0] == 3: + # press left button + if data[1] == 1: + # additional callbacks + if "L" in self._additional_callbacks: + self._additional_callbacks["L"] + # right button is for reset + if data[1] == 2: + # reset layer + self.reset() + # additional callbacks + if "R" in self._additional_callbacks: + self._additional_callbacks["R"] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se3_spacemouse.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se3_spacemouse.py new file mode 100644 index 00000000000..46501994da8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/se3_spacemouse.py @@ -0,0 +1,180 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spacemouse controller for SE(3) control.""" + +import hid +import numpy as np +import threading +import time +from collections.abc import Callable +from scipy.spatial.transform import Rotation + +from ..device_base import DeviceBase +from .utils import convert_buffer + + +class Se3SpaceMouse(DeviceBase): + """A space-mouse controller for sending SE(3) commands as delta poses. + + This class implements a space-mouse controller to provide commands to a robotic arm with a gripper. + It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms [1]. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Note: + The interface finds and uses the first supported device connected to the computer. + + Currently tested for following devices: + + - SpaceMouse Compact: https://3dconnexion.com/de/product/spacemouse-compact/ + + .. _HID-API: https://github.com/libusb/hidapi + + """ + + def __init__(self, pos_sensitivity: float = 0.4, rot_sensitivity: float = 0.8): + """Initialize the space-mouse layer. + + Args: + pos_sensitivity: Magnitude of input position command scaling. Defaults to 0.4. + rot_sensitivity: Magnitude of scale input rotation commands scaling. Defaults to 0.8. + """ + # store inputs + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + # acquire device interface + self._device = hid.device() + self._find_device() + # read rotations + self._read_rotation = False + + # command buffers + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + # dictionary for additional callbacks + self._additional_callbacks = dict() + # run a thread for listening to device updates + self._thread = threading.Thread(target=self._run_device) + self._thread.daemon = True + self._thread.start() + + def __del__(self): + """Destructor for the class.""" + self._thread.join() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Spacemouse Controller for SE(3): {self.__class__.__name__}\n" + msg += f"\tManufacturer: {self._device.get_manufacturer_string()}\n" + msg += f"\tProduct: {self._device.get_product_string()}\n" + msg += "\t----------------------------------------------\n" + msg += "\tRight button: reset command\n" + msg += "\tLeft button: toggle gripper command (open/close)\n" + msg += "\tMove mouse laterally: move arm horizontally in x-y plane\n" + msg += "\tMove mouse vertically: move arm vertically\n" + msg += "\tTwist mouse about an axis: rotate arm about a corresponding axis" + return msg + + """ + Operations + """ + + def reset(self): + # default flags + self._close_gripper = False + self._delta_pos = np.zeros(3) # (x, y, z) + self._delta_rot = np.zeros(3) # (roll, pitch, yaw) + + def add_callback(self, key: str, func: Callable): + # check keys supported by callback + if key not in ["L", "R"]: + raise ValueError(f"Only left (L) and right (R) buttons supported. Provided: {key}.") + # TODO: Improve this to allow multiple buttons on same key. + self._additional_callbacks[key] = func + + def advance(self) -> tuple[np.ndarray, bool]: + """Provides the result from spacemouse event state. + + Returns: + A tuple containing the delta pose command and gripper commands. + """ + rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec() + # if new command received, reset event flag to False until keyboard updated. + return np.concatenate([self._delta_pos, rot_vec]), self._close_gripper + + """ + Internal helpers. + """ + + def _find_device(self): + """Find the device connected to computer.""" + found = False + # implement a timeout for device search + for _ in range(5): + for device in hid.enumerate(): + if ( + device["product_string"] == "SpaceMouse Compact" + or device["product_string"] == "SpaceMouse Wireless" + ): + # set found flag + found = True + vendor_id = device["vendor_id"] + product_id = device["product_id"] + # connect to the device + self._device.close() + self._device.open(vendor_id, product_id) + # check if device found + if not found: + time.sleep(1.0) + else: + break + # no device found: return false + if not found: + raise OSError("No device found by SpaceMouse. Is the device connected?") + + def _run_device(self): + """Listener thread that keeps pulling new messages.""" + # keep running + while True: + # read the device data + data = self._device.read(7) + if data is not None: + # readings from 6-DoF sensor + if data[0] == 1: + self._delta_pos[1] = self.pos_sensitivity * convert_buffer(data[1], data[2]) + self._delta_pos[0] = self.pos_sensitivity * convert_buffer(data[3], data[4]) + self._delta_pos[2] = self.pos_sensitivity * convert_buffer(data[5], data[6]) * -1.0 + elif data[0] == 2 and not self._read_rotation: + self._delta_rot[1] = self.rot_sensitivity * convert_buffer(data[1], data[2]) + self._delta_rot[0] = self.rot_sensitivity * convert_buffer(data[3], data[4]) + self._delta_rot[2] = self.rot_sensitivity * convert_buffer(data[5], data[6]) * -1.0 + # readings from the side buttons + elif data[0] == 3: + # press left button + if data[1] == 1: + # close gripper + self._close_gripper = not self._close_gripper + # additional callbacks + if "L" in self._additional_callbacks: + self._additional_callbacks["L"]() + # right button is for reset + if data[1] == 2: + # reset layer + self.reset() + # additional callbacks + if "R" in self._additional_callbacks: + self._additional_callbacks["R"]() + if data[1] == 3: + self._read_rotation = not self._read_rotation diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/utils.py new file mode 100644 index 00000000000..e6b970fed49 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/devices/spacemouse/utils.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions for SpaceMouse.""" + +# MIT License +# +# Copyright (c) 2022 Stanford Vision and Learning Lab and UT Robot Perception and Learning Lab +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + + +def convert_buffer(b1, b2): + """Converts raw SpaceMouse readings to commands. + + Args: + b1: 8-bit byte + b2: 8-bit byte + + Returns: + Scaled value from Space-mouse message + """ + return _scale_to_control(_to_int16(b1, b2)) + + +""" +Private methods. +""" + + +def _to_int16(y1, y2): + """Convert two 8 bit bytes to a signed 16 bit integer. + + Args: + y1: 8-bit byte + y2: 8-bit byte + + Returns: + 16-bit integer + """ + x = (y1) | (y2 << 8) + if x >= 32768: + x = -(65536 - x) + return x + + +def _scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0): + """Normalize raw HID readings to target range. + + Args: + x: Raw reading from HID + axis_scale: (Inverted) scaling factor for mapping raw input value + min_v: Minimum limit after scaling + max_v: Maximum limit after scaling + + Returns: + Clipped, scaled input from HID + """ + x = x / axis_scale + return min(max(x, min_v), max_v) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/__init__.py new file mode 100644 index 00000000000..b5ef2aad6d1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/__init__.py @@ -0,0 +1,62 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for environment definitions. + +Environments define the interface between the agent and the simulation. +In the simplest case, the environment provides the agent with the current +observations and executes the actions provided by the agent. However, the +environment can also provide additional information such as the current +reward, done flag, and information about the current episode. + +There are two types of environment designing workflows: + +* **Manager-based**: The environment is decomposed into individual components (or managers) + for different aspects (such as computing observations, applying actions, and applying + randomization. The users mainly configure the managers and the environment coordinates the + managers and calls their functions. +* **Direct**: The user implements all the necessary functionality directly into a single class + directly without the need for additional managers. + +Based on these workflows, there are the following environment classes for single and multi-agent RL: + +**Single-Agent RL:** + +* :class:`ManagerBasedEnv`: The manager-based workflow base environment which only provides the + agent with the current observations and executes the actions provided by the agent. +* :class:`ManagerBasedRLEnv`: The manager-based workflow RL task environment which besides the + functionality of the base environment also provides additional Markov Decision Process (MDP) + related information such as the current reward, done flag, and information. +* :class:`DirectRLEnv`: The direct workflow RL task environment which provides implementations for + implementing scene setup, computing dones, performing resets, and computing reward and observation. + +**Multi-Agent RL (MARL):** + +* :class:`DirectMARLEnv`: The direct workflow MARL task environment which provides implementations for + implementing scene setup, computing dones, performing resets, and computing reward and observation. + +For more information about the workflow design patterns, see the `Task Design Workflows`_ section. + +.. _`Task Design Workflows`: https://isaac-sim.github.io/IsaacLab/source/features/task_workflows.html +""" + +from . import mdp, ui +from .common import VecEnvObs, VecEnvStepReturn, ViewerCfg +from .direct_marl_env import DirectMARLEnv +from .direct_marl_env_cfg import DirectMARLEnvCfg +from .direct_rl_env import DirectRLEnv +from .direct_rl_env_cfg import DirectRLEnvCfg +from .manager_based_env import ManagerBasedEnv +from .manager_based_env_cfg import ManagerBasedEnvCfg +from .manager_based_rl_env import ManagerBasedRLEnv +from .manager_based_rl_env_cfg import ManagerBasedRLEnvCfg +from .manager_based_rl_mimic_env import ManagerBasedRLMimicEnv +from .mimic_env_cfg import * +from .utils.marl import multi_agent_to_single_agent, multi_agent_with_one_agent diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/common.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/common.py new file mode 100644 index 00000000000..3fec4e5ead2 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/common.py @@ -0,0 +1,151 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import torch +from typing import Dict, Literal, TypeVar + +from isaaclab.utils import configclass + +## +# Configuration. +## + + +@configclass +class ViewerCfg: + """Configuration of the scene viewport camera.""" + + eye: tuple[float, float, float] = (7.5, 7.5, 7.5) + """Initial camera position (in m). Default is (7.5, 7.5, 7.5).""" + + lookat: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Initial camera target position (in m). Default is (0.0, 0.0, 0.0).""" + + cam_prim_path: str = "/OmniverseKit_Persp" + """The camera prim path to record images from. Default is "/OmniverseKit_Persp", + which is the default camera in the viewport. + """ + + resolution: tuple[int, int] = (1280, 720) + """The resolution (width, height) of the camera specified using :attr:`cam_prim_path`. + Default is (1280, 720). + """ + + origin_type: Literal["world", "env", "asset_root", "asset_body"] = "world" + """The frame in which the camera position (eye) and target (lookat) are defined in. Default is "world". + + Available options are: + + * ``"world"``: The origin of the world. + * ``"env"``: The origin of the environment defined by :attr:`env_index`. + * ``"asset_root"``: The center of the asset defined by :attr:`asset_name` in environment :attr:`env_index`. + * ``"asset_body"``: The center of the body defined by :attr:`body_name` in asset defined by :attr:`asset_name` in environment :attr:`env_index`. + """ + + env_index: int = 0 + """The environment index for frame origin. Default is 0. + + This quantity is only effective if :attr:`origin` is set to "env" or "asset_root". + """ + + asset_name: str | None = None + """The asset name in the interactive scene for the frame origin. Default is None. + + This quantity is only effective if :attr:`origin` is set to "asset_root". + """ + + body_name: str | None = None + """The name of the body in :attr:`asset_name` in the interactive scene for the frame origin. Default is None. + + This quantity is only effective if :attr:`origin` is set to "asset_body". + """ + + +## +# Types. +## + +SpaceType = TypeVar("SpaceType", gym.spaces.Space, int, set, tuple, list, dict) +"""A sentinel object to indicate a valid space type to specify states, observations and actions.""" + +VecEnvObs = Dict[str, torch.Tensor | Dict[str, torch.Tensor]] +"""Observation returned by the environment. + +The observations are stored in a dictionary. The keys are the group to which the observations belong. +This is useful for various setups such as reinforcement learning with asymmetric actor-critic or +multi-agent learning. For non-learning paradigms, this may include observations for different components +of a system. + +Within each group, the observations can be stored either as a dictionary with keys as the names of each +observation term in the group, or a single tensor obtained from concatenating all the observation terms. +For example, for asymmetric actor-critic, the observation for the actor and the critic can be accessed +using the keys ``"policy"`` and ``"critic"`` respectively. + +Note: + By default, most learning frameworks deal with default and privileged observations in different ways. + This handling must be taken care of by the wrapper around the :class:`ManagerBasedRLEnv` instance. + + For included frameworks (RSL-RL, RL-Games, skrl), the observations must have the key "policy". In case, + the key "critic" is also present, then the critic observations are taken from the "critic" group. + Otherwise, they are the same as the "policy" group. + +""" + +VecEnvStepReturn = tuple[VecEnvObs, torch.Tensor, torch.Tensor, torch.Tensor, dict] +"""The environment signals processed at the end of each step. + +The tuple contains batched information for each sub-environment. The information is stored in the following order: + +1. **Observations**: The observations from the environment. +2. **Rewards**: The rewards from the environment. +3. **Terminated Dones**: Whether the environment reached a terminal state, such as task success or robot falling etc. +4. **Timeout Dones**: Whether the environment reached a timeout state, such as end of max episode length. +5. **Extras**: A dictionary containing additional information from the environment. +""" + +AgentID = TypeVar("AgentID") +"""Unique identifier for an agent within a multi-agent environment. + +The identifier has to be an immutable object, typically a string (e.g.: ``"agent_0"``). +""" + +ObsType = TypeVar("ObsType", torch.Tensor, Dict[str, torch.Tensor]) +"""A sentinel object to indicate the data type of the observation. +""" + +ActionType = TypeVar("ActionType", torch.Tensor, Dict[str, torch.Tensor]) +"""A sentinel object to indicate the data type of the action. +""" + +StateType = TypeVar("StateType", torch.Tensor, dict) +"""A sentinel object to indicate the data type of the state. +""" + +EnvStepReturn = tuple[ + Dict[AgentID, ObsType], + Dict[AgentID, torch.Tensor], + Dict[AgentID, torch.Tensor], + Dict[AgentID, torch.Tensor], + Dict[AgentID, dict], +] +"""The environment signals processed at the end of each step. + +The tuple contains batched information for each sub-environment (keyed by the agent ID). +The information is stored in the following order: + +1. **Observations**: The observations from the environment. +2. **Rewards**: The rewards from the environment. +3. **Terminated Dones**: Whether the environment reached a terminal state, such as task success or robot falling etc. +4. **Timeout Dones**: Whether the environment reached a timeout state, such as end of max episode length. +5. **Extras**: A dictionary containing additional information from the environment. +""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env.py new file mode 100644 index 00000000000..71df6d9e72d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env.py @@ -0,0 +1,742 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import builtins +import gymnasium as gym +import inspect +import math +import numpy as np +import torch +import weakref +from abc import abstractmethod +from collections.abc import Sequence +from dataclasses import MISSING +from typing import Any, ClassVar + +import isaacsim.core.utils.torch as torch_utils +import omni.kit.app +import omni.log +from isaacsim.core.version import get_version + +from isaaclab.managers import EventManager +from isaaclab.scene import InteractiveScene +from isaaclab.sim import SimulationContext +from isaaclab.utils.noise import NoiseModel +from isaaclab.utils.timer import Timer + +from .common import ActionType, AgentID, EnvStepReturn, ObsType, StateType +from .direct_marl_env_cfg import DirectMARLEnvCfg +from .ui import ViewportCameraController +from .utils.spaces import sample_space, spec_to_gym_space + + +class DirectMARLEnv(gym.Env): + """The superclass for the direct workflow to design multi-agent environments. + + This class implements the core functionality for multi-agent reinforcement learning (MARL) + environments. It is designed to be used with any RL library. The class is designed + to be used with vectorized environments, i.e., the environment is expected to be run + in parallel with multiple sub-environments. + + The design of this class is based on the PettingZoo Parallel API. + While the environment itself is implemented as a vectorized environment, we do not + inherit from :class:`pettingzoo.ParallelEnv` or :class:`gym.vector.VectorEnv`. This is mainly + because the class adds various attributes and methods that are inconsistent with them. + + Note: + For vectorized environments, it is recommended to **only** call the :meth:`reset` + method once before the first call to :meth:`step`, i.e. after the environment is created. + After that, the :meth:`step` function handles the reset of terminated sub-environments. + This is because the simulator does not support resetting individual sub-environments + in a vectorized environment. + + """ + + metadata: ClassVar[dict[str, Any]] = { + "render_modes": [None, "human", "rgb_array"], + "isaac_sim_version": get_version(), + } + """Metadata for the environment.""" + + def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwargs): + """Initialize the environment. + + Args: + cfg: The configuration object for the environment. + render_mode: The render mode for the environment. Defaults to None, which + is similar to ``"human"``. + + Raises: + RuntimeError: If a simulation context already exists. The environment must always create one + since it configures the simulation context and controls the simulation. + """ + # check that the config is valid + cfg.validate() + # store inputs to class + self.cfg = cfg + # store the render mode + self.render_mode = render_mode + # initialize internal variables + self._is_closed = False + + # set the seed for the environment + if self.cfg.seed is not None: + self.cfg.seed = self.seed(self.cfg.seed) + else: + omni.log.warn("Seed not set for the environment. The environment creation may not be deterministic.") + + # create a simulation context to control the simulator + if SimulationContext.instance() is None: + self.sim: SimulationContext = SimulationContext(self.cfg.sim) + else: + raise RuntimeError("Simulation context already exists. Cannot create a new one.") + + # make sure torch is running on the correct device + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + # print useful information + print("[INFO]: Base environment:") + print(f"\tEnvironment device : {self.device}") + print(f"\tEnvironment seed : {self.cfg.seed}") + print(f"\tPhysics step-size : {self.physics_dt}") + print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}") + print(f"\tEnvironment step-size : {self.step_dt}") + + if self.cfg.sim.render_interval < self.cfg.decimation: + msg = ( + f"The render interval ({self.cfg.sim.render_interval}) is smaller than the decimation " + f"({self.cfg.decimation}). Multiple render calls will happen for each environment step." + "If this is not intended, set the render interval to be equal to the decimation." + ) + omni.log.warn(msg) + + # generate scene + with Timer("[INFO]: Time taken for scene creation", "scene_creation"): + self.scene = InteractiveScene(self.cfg.scene) + self._setup_scene() + print("[INFO]: Scene manager: ", self.scene) + + # set up camera viewport controller + # viewport is not available in other rendering modes so the function will throw a warning + # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for + # non-rendering modes. + if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) + else: + self.viewport_camera_controller = None + + # create event manager + # note: this is needed here (rather than after simulation play) to allow USD-related randomization events + # that must happen before the simulation starts. Example: randomizing mesh scale + if self.cfg.events: + self.event_manager = EventManager(self.cfg.events, self) + + # apply USD-related randomization events + if "prestartup" in self.event_manager.available_modes: + self.event_manager.apply(mode="prestartup") + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + # note: when started in extension mode, first call sim.reset_async() and then initialize the managers + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) + + # check if debug visualization is has been implemented by the environment + source_code = inspect.getsource(self._set_debug_vis_impl) + self.has_debug_vis_implementation = "NotImplementedError" not in source_code + self._debug_vis_handle = None + + # extend UI elements + # we need to do this here after all the managers are initialized + # this is because they dictate the sensors and commands right now + if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") + else: + # if no window, then we don't need to store the window + self._window = None + + # allocate dictionary to store metrics + self.extras = {agent: {} for agent in self.cfg.possible_agents} + + # initialize data and constants + # -- counter for simulation steps + self._sim_step_counter = 0 + # -- counter for curriculum + self.common_step_counter = 0 + # -- init buffers + self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + self.reset_buf = torch.zeros(self.num_envs, dtype=torch.bool, device=self.sim.device) + + # setup the observation, state and action spaces + self._configure_env_spaces() + + # setup noise cfg for adding action and observation noise + if self.cfg.action_noise_model: + self._action_noise_model: dict[AgentID, NoiseModel] = { + agent: noise_model.class_type(noise_model, num_envs=self.num_envs, device=self.device) + for agent, noise_model in self.cfg.action_noise_model.items() + if noise_model is not None + } + if self.cfg.observation_noise_model: + self._observation_noise_model: dict[AgentID, NoiseModel] = { + agent: noise_model.class_type(noise_model, num_envs=self.num_envs, device=self.device) + for agent, noise_model in self.cfg.observation_noise_model.items() + if noise_model is not None + } + + # perform events at the start of the simulation + if self.cfg.events: + # we print it here to make the logging consistent + print("[INFO] Event Manager: ", self.event_manager) + + if "startup" in self.event_manager.available_modes: + self.event_manager.apply(mode="startup") + + # print the environment information + print("[INFO]: Completed setting up the environment...") + + def __del__(self): + """Cleanup for the environment.""" + self.close() + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """The number of instances of the environment that are running.""" + return self.scene.num_envs + + @property + def num_agents(self) -> int: + """Number of current agents. + + The number of current agents may change as the environment progresses (e.g.: agents can be added or removed). + """ + return len(self.agents) + + @property + def max_num_agents(self) -> int: + """Number of all possible agents the environment can generate. + + This value remains constant as the environment progresses. + """ + return len(self.possible_agents) + + @property + def unwrapped(self) -> DirectMARLEnv: + """Get the unwrapped environment underneath all the layers of wrappers.""" + return self + + @property + def physics_dt(self) -> float: + """The physics time-step (in s). + + This is the lowest time-decimation at which the simulation is happening. + """ + return self.cfg.sim.dt + + @property + def step_dt(self) -> float: + """The environment stepping time-step (in s). + + This is the time-step at which the environment steps forward. + """ + return self.cfg.sim.dt * self.cfg.decimation + + @property + def device(self): + """The device on which the environment is running.""" + return self.sim.device + + @property + def max_episode_length_s(self) -> float: + """Maximum episode length in seconds.""" + return self.cfg.episode_length_s + + @property + def max_episode_length(self): + """The maximum episode length in steps adjusted from s.""" + return math.ceil(self.max_episode_length_s / (self.cfg.sim.dt * self.cfg.decimation)) + + """ + Space methods + """ + + def observation_space(self, agent: AgentID) -> gym.Space: + """Get the observation space for the specified agent. + + Returns: + The agent's observation space. + """ + return self.observation_spaces[agent] + + def action_space(self, agent: AgentID) -> gym.Space: + """Get the action space for the specified agent. + + Returns: + The agent's action space. + """ + return self.action_spaces[agent] + + """ + Operations. + """ + + def reset( + self, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[AgentID, ObsType], dict[AgentID, dict]]: + """Resets all the environments and returns observations. + + Args: + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + options: Additional information to specify how the environment is reset. Defaults to None. + + Note: + This argument is used for compatibility with Gymnasium environment definition. + + Returns: + A tuple containing the observations and extras (keyed by the agent ID). + """ + # set the seed + if seed is not None: + self.seed(seed) + + # reset state of scene + indices = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + self._reset_idx(indices) + + # update observations and the list of current agents (sorted as in possible_agents) + self.obs_dict = self._get_observations() + self.agents = [agent for agent in self.possible_agents if agent in self.obs_dict] + + # return observations + return self.obs_dict, self.extras + + def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: + """Execute one time-step of the environment's dynamics. + + The environment steps forward at a fixed time-step, while the physics simulation is decimated at a + lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured + independently using the :attr:`DirectMARLEnvCfg.decimation` (number of simulation steps per environment step) + and the :attr:`DirectMARLEnvCfg.sim.physics_dt` (physics time-step). Based on these parameters, the environment + time-step is computed as the product of the two. + + This function performs the following steps: + + 1. Pre-process the actions before stepping through the physics. + 2. Apply the actions to the simulator and step through the physics in a decimated manner. + 3. Compute the reward and done signals. + 4. Reset environments that have terminated or reached the maximum episode length. + 5. Apply interval events if they are enabled. + 6. Compute observations. + + Args: + actions: The actions to apply on the environment (keyed by the agent ID). + Shape of individual tensors is (num_envs, action_dim). + + Returns: + A tuple containing the observations, rewards, resets (terminated and truncated) and extras (keyed by the agent ID). + """ + actions = {agent: action.to(self.device) for agent, action in actions.items()} + + # add action noise + if self.cfg.action_noise_model: + for agent, action in actions.items(): + if agent in self._action_noise_model: + actions[agent] = self._action_noise_model[agent].apply(action) + # process actions + self._pre_physics_step(actions) + + # check if we need to do rendering within the physics loop + # note: checked here once to avoid multiple checks within the loop + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self._apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # post-step: + # -- update env counters (used for curriculum generation) + self.episode_length_buf += 1 # step in current episode (per env) + self.common_step_counter += 1 # total step (common for all envs) + + self.terminated_dict, self.time_out_dict = self._get_dones() + self.reset_buf[:] = math.prod(self.terminated_dict.values()) | math.prod(self.time_out_dict.values()) + self.reward_dict = self._get_rewards() + + # -- reset envs that terminated/timed-out and log the episode information + reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(reset_env_ids) > 0: + self._reset_idx(reset_env_ids) + + # post-step: step interval event + if self.cfg.events: + if "interval" in self.event_manager.available_modes: + self.event_manager.apply(mode="interval", dt=self.step_dt) + + # update observations and the list of current agents (sorted as in possible_agents) + self.obs_dict = self._get_observations() + self.agents = [agent for agent in self.possible_agents if agent in self.obs_dict] + + # add observation noise + # note: we apply no noise to the state space (since it is used for centralized training or critic networks) + if self.cfg.observation_noise_model: + for agent, obs in self.obs_dict.items(): + if agent in self._observation_noise_model: + self.obs_dict[agent] = self._observation_noise_model[agent].apply(obs) + + # return observations, rewards, resets and extras + return self.obs_dict, self.reward_dict, self.terminated_dict, self.time_out_dict, self.extras + + def state(self) -> StateType | None: + """Returns the state for the environment. + + The state-space is used for centralized training or asymmetric actor-critic architectures. It is configured + using the :attr:`DirectMARLEnvCfg.state_space` parameter. + + Returns: + The states for the environment, or None if :attr:`DirectMARLEnvCfg.state_space` parameter is zero. + """ + if not self.cfg.state_space: + return None + # concatenate and return the observations as state + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + if isinstance(self.cfg.state_space, int) and self.cfg.state_space < 0: + self.state_buf = torch.cat( + [self.obs_dict[agent].reshape(self.num_envs, -1) for agent in self.cfg.possible_agents], dim=-1 + ) + # compute and return custom environment state + else: + self.state_buf = self._get_states() + return self.state_buf + + @staticmethod + def seed(seed: int = -1) -> int: + """Set the seed for the environment. + + Args: + seed: The seed for random generator. Defaults to -1. + + Returns: + The seed used for random generator. + """ + # set seed for replicator + try: + import omni.replicator.core as rep + + rep.set_global_seed(seed) + except ModuleNotFoundError: + pass + # set seed for torch and other libraries + return torch_utils.set_seed(seed) + + def render(self, recompute: bool = False) -> np.ndarray | None: + """Run rendering without stepping through the physics. + + By convention, if mode is: + + - **human**: Render to the current display and return nothing. Usually for human consumption. + - **rgb_array**: Return an numpy.ndarray with shape (x, y, 3), representing RGB values for an + x-by-y pixel image, suitable for turning into a video. + + Args: + recompute: Whether to force a render even if the simulator has already rendered the scene. + Defaults to False. + + Returns: + The rendered image as a numpy array if mode is "rgb_array". Otherwise, returns None. + + Raises: + RuntimeError: If mode is set to "rgb_data" and simulation render mode does not support it. + In this case, the simulation render mode must be set to ``RenderMode.PARTIAL_RENDERING`` + or ``RenderMode.FULL_RENDERING``. + NotImplementedError: If an unsupported rendering mode is specified. + """ + # run a rendering step of the simulator + # if we have rtx sensors, we do not need to render again sin + if not self.sim.has_rtx_sensors() and not recompute: + self.sim.render() + # decide the rendering mode + if self.render_mode == "human" or self.render_mode is None: + return None + elif self.render_mode == "rgb_array": + # check that if any render could have happened + if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + raise RuntimeError( + f"Cannot render '{self.render_mode}' when the simulation render mode is" + f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." + " If running headless, make sure --enable_cameras is set." + ) + # create the annotator if it does not exist + if not hasattr(self, "_rgb_annotator"): + import omni.replicator.core as rep + + # create render product + self._render_product = rep.create.render_product( + self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution + ) + # create rgb annotator -- used to read data from the render product + self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") + self._rgb_annotator.attach([self._render_product]) + # obtain the rgb data + rgb_data = self._rgb_annotator.get_data() + # convert to numpy array + rgb_data = np.frombuffer(rgb_data, dtype=np.uint8).reshape(*rgb_data.shape) + # return the rgb data + # note: initially the renderer is warming up and returns empty data + if rgb_data.size == 0: + return np.zeros((self.cfg.viewer.resolution[1], self.cfg.viewer.resolution[0], 3), dtype=np.uint8) + else: + return rgb_data[:, :, :3] + else: + raise NotImplementedError( + f"Render mode '{self.render_mode}' is not supported. Please use: {self.metadata['render_modes']}." + ) + + def close(self): + """Cleanup for the environment.""" + if not self._is_closed: + # close entities related to the environment + # note: this is order-sensitive to avoid any dangling references + if self.cfg.events: + del self.event_manager + del self.scene + if self.viewport_camera_controller is not None: + del self.viewport_camera_controller + # clear callbacks and instance + self.sim.clear_all_callbacks() + self.sim.clear_instance() + # destroy the window + if self._window is not None: + self._window = None + # update closing status + self._is_closed = True + + """ + Operations - Debug Visualization. + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Toggles the environment debug visualization. + + Args: + debug_vis: Whether to visualize the environment debug visualization. + + Returns: + Whether the debug visualization was successfully set. False if the environment + does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + """ + Helper functions. + """ + + def _configure_env_spaces(self): + """Configure the spaces for the environment.""" + self.agents = self.cfg.possible_agents + self.possible_agents = self.cfg.possible_agents + + # show deprecation message and overwrite configuration + if self.cfg.num_actions is not None: + omni.log.warn("DirectMARLEnvCfg.num_actions is deprecated. Use DirectMARLEnvCfg.action_spaces instead.") + if isinstance(self.cfg.action_spaces, type(MISSING)): + self.cfg.action_spaces = self.cfg.num_actions + if self.cfg.num_observations is not None: + omni.log.warn( + "DirectMARLEnvCfg.num_observations is deprecated. Use DirectMARLEnvCfg.observation_spaces instead." + ) + if isinstance(self.cfg.observation_spaces, type(MISSING)): + self.cfg.observation_spaces = self.cfg.num_observations + if self.cfg.num_states is not None: + omni.log.warn("DirectMARLEnvCfg.num_states is deprecated. Use DirectMARLEnvCfg.state_space instead.") + if isinstance(self.cfg.state_space, type(MISSING)): + self.cfg.state_space = self.cfg.num_states + + # set up observation and action spaces + self.observation_spaces = { + agent: spec_to_gym_space(self.cfg.observation_spaces[agent]) for agent in self.cfg.possible_agents + } + self.action_spaces = { + agent: spec_to_gym_space(self.cfg.action_spaces[agent]) for agent in self.cfg.possible_agents + } + + # set up state space + if not self.cfg.state_space: + self.state_space = None + if isinstance(self.cfg.state_space, int) and self.cfg.state_space < 0: + self.state_space = gym.spaces.flatten_space( + gym.spaces.Tuple([self.observation_spaces[agent] for agent in self.cfg.possible_agents]) + ) + else: + self.state_space = spec_to_gym_space(self.cfg.state_space) + + # instantiate actions (needed for tasks for which the observations computation is dependent on the actions) + self.actions = { + agent: sample_space(self.action_spaces[agent], self.sim.device, batch_size=self.num_envs, fill_value=0) + for agent in self.cfg.possible_agents + } + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset environments based on specified indices. + + Args: + env_ids: List of environment ids which must be reset + """ + self.scene.reset(env_ids) + + # apply events such as randomization for environments that need a reset + if self.cfg.events: + if "reset" in self.event_manager.available_modes: + env_step_count = self._sim_step_counter // self.cfg.decimation + self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) + + # reset noise models + if self.cfg.action_noise_model: + for noise_model in self._action_noise_model.values(): + noise_model.reset(env_ids) + if self.cfg.observation_noise_model: + for noise_model in self._observation_noise_model.values(): + noise_model.reset(env_ids) + + # reset the episode length buffer + self.episode_length_buf[env_ids] = 0 + + """ + Implementation-specific functions. + """ + + def _setup_scene(self): + """Setup the scene for the environment. + + This function is responsible for creating the scene objects and setting up the scene for the environment. + The scene creation can happen through :class:`isaaclab.scene.InteractiveSceneCfg` or through + directly creating the scene objects and registering them with the scene manager. + + We leave the implementation of this function to the derived classes. If the environment does not require + any explicit scene setup, the function can be left empty. + """ + pass + + @abstractmethod + def _pre_physics_step(self, actions: dict[AgentID, ActionType]): + """Pre-process actions before stepping through the physics. + + This function is responsible for pre-processing the actions before stepping through the physics. + It is called before the physics stepping (which is decimated). + + Args: + actions: The actions to apply on the environment (keyed by the agent ID). + Shape of individual tensors is (num_envs, action_dim). + """ + raise NotImplementedError(f"Please implement the '_pre_physics_step' method for {self.__class__.__name__}.") + + @abstractmethod + def _apply_action(self): + """Apply actions to the simulator. + + This function is responsible for applying the actions to the simulator. It is called at each + physics time-step. + """ + raise NotImplementedError(f"Please implement the '_apply_action' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_observations(self) -> dict[AgentID, ObsType]: + """Compute and return the observations for the environment. + + Returns: + The observations for the environment (keyed by the agent ID). + """ + raise NotImplementedError(f"Please implement the '_get_observations' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_states(self) -> StateType: + """Compute and return the states for the environment. + + This method is only called (and therefore has to be implemented) when the :attr:`DirectMARLEnvCfg.state_space` + parameter is not a number less than or equal to zero. + + Returns: + The states for the environment. + """ + raise NotImplementedError(f"Please implement the '_get_states' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_rewards(self) -> dict[AgentID, torch.Tensor]: + """Compute and return the rewards for the environment. + + Returns: + The rewards for the environment (keyed by the agent ID). + Shape of individual tensors is (num_envs,). + """ + raise NotImplementedError(f"Please implement the '_get_rewards' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_dones(self) -> tuple[dict[AgentID, torch.Tensor], dict[AgentID, torch.Tensor]]: + """Compute and return the done flags for the environment. + + Returns: + A tuple containing the done flags for termination and time-out (keyed by the agent ID). + Shape of individual tensors is (num_envs,). + """ + raise NotImplementedError(f"Please implement the '_get_dones' method for {self.__class__.__name__}.") + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env_cfg.py new file mode 100644 index 00000000000..04f47ef0303 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_marl_env_cfg.py @@ -0,0 +1,232 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.devices.openxr import XrCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass +from isaaclab.utils.noise import NoiseModelCfg + +from .common import AgentID, SpaceType, ViewerCfg +from .ui import BaseEnvWindow + + +@configclass +class DirectMARLEnvCfg: + """Configuration for a MARL environment defined with the direct workflow. + + Please refer to the :class:`isaaclab.envs.direct_marl_env.DirectMARLEnv` class for more details. + """ + + # simulation settings + viewer: ViewerCfg = ViewerCfg() + """Viewer configuration. Default is ViewerCfg().""" + + sim: SimulationCfg = SimulationCfg() + """Physics simulation configuration. Default is SimulationCfg().""" + + # ui settings + ui_window_class_type: type | None = BaseEnvWindow + """The class type of the UI window. Default is None. + + If None, then no UI window is created. + + Note: + If you want to make your own UI window, you can create a class that inherits from + from :class:`isaaclab.envs.ui.base_env_window.BaseEnvWindow`. Then, you can set + this attribute to your class type. + """ + + # general settings + seed: int | None = None + """The seed for the random number generator. Defaults to None, in which case the seed is not set. + + Note: + The seed is set at the beginning of the environment initialization. This ensures that the environment + creation is deterministic and behaves similarly across different runs. + """ + + decimation: int = MISSING + """Number of control action updates @ sim dt per policy dt. + + For instance, if the simulation dt is 0.01s and the policy dt is 0.1s, then the decimation is 10. + This means that the control action is updated every 10 simulation steps. + """ + + is_finite_horizon: bool = False + """Whether the learning task is treated as a finite or infinite horizon problem for the agent. + Defaults to False, which means the task is treated as an infinite horizon problem. + + This flag handles the subtleties of finite and infinite horizon tasks: + + * **Finite horizon**: no penalty or bootstrapping value is required by the the agent for + running out of time. However, the environment still needs to terminate the episode after the + time limit is reached. + * **Infinite horizon**: the agent needs to bootstrap the value of the state at the end of the episode. + This is done by sending a time-limit (or truncated) done signal to the agent, which triggers this + bootstrapping calculation. + + If True, then the environment is treated as a finite horizon problem and no time-out (or truncated) done signal + is sent to the agent. If False, then the environment is treated as an infinite horizon problem and a time-out + (or truncated) done signal is sent to the agent. + + Note: + The base :class:`ManagerBasedRLEnv` class does not use this flag directly. It is used by the environment + wrappers to determine what type of done signal to send to the corresponding learning agent. + """ + + episode_length_s: float = MISSING + """Duration of an episode (in seconds). + + Based on the decimation rate and physics time step, the episode length is calculated as: + + .. code-block:: python + + episode_length_steps = ceil(episode_length_s / (decimation_rate * physics_time_step)) + + For example, if the decimation rate is 10, the physics time step is 0.01, and the episode length is 10 seconds, + then the episode length in steps is 100. + """ + + # environment settings + scene: InteractiveSceneCfg = MISSING + """Scene settings. + + Please refer to the :class:`isaaclab.scene.InteractiveSceneCfg` class for more details. + """ + + events: object = None + """Event settings. Defaults to None, in which case no events are applied through the event manager. + + Please refer to the :class:`isaaclab.managers.EventManager` class for more details. + """ + + observation_spaces: dict[AgentID, SpaceType] = MISSING + """Observation space definition for each agent. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_observations: dict[AgentID, int] | None = None + """The dimension of the observation space for each agent. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectMARLEnvCfg.observation_spaces` instead. + """ + + state_space: SpaceType = MISSING + """State space definition. + + The following values are supported: + + * -1: All the observations from the different agents are automatically concatenated. + * 0: No state-space will be constructed (`state_space` is None). + This is useful to save computational resources when the algorithm to be trained does not need it. + * greater than 0: Custom state-space dimension to be provided by the task implementation. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_states: int | None = None + """The dimension of the state space from each environment instance. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectMARLEnvCfg.state_space` instead. + """ + + observation_noise_model: dict[AgentID, NoiseModelCfg | None] | None = None + """The noise model to apply to the computed observations from the environment. Default is None, which means no noise is added. + + Please refer to the :class:`isaaclab.utils.noise.NoiseModel` class for more details. + """ + + action_spaces: dict[AgentID, SpaceType] = MISSING + """Action space definition for each agent. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_actions: dict[AgentID, int] | None = None + """The dimension of the action space for each agent. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectMARLEnvCfg.action_spaces` instead. + """ + + action_noise_model: dict[AgentID, NoiseModelCfg | None] | None = None + """The noise model applied to the actions provided to the environment. Default is None, which means no noise is added. + + Please refer to the :class:`isaaclab.utils.noise.NoiseModel` class for more details. + """ + + possible_agents: list[AgentID] = MISSING + """A list of all possible agents the environment could generate. + + The contents of the list cannot be modified during the entire training process. + """ + + xr: XrCfg | None = None + """Configuration for viewing and interacting with the environment through an XR device.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env.py new file mode 100644 index 00000000000..f900a6cf51b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env.py @@ -0,0 +1,684 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import builtins +import gymnasium as gym +import inspect +import math +import numpy as np +import torch +import weakref +from abc import abstractmethod +from collections.abc import Sequence +from dataclasses import MISSING +from typing import Any, ClassVar + +import isaacsim.core.utils.torch as torch_utils +import omni.kit.app +import omni.log +from isaacsim.core.simulation_manager import SimulationManager +from isaacsim.core.version import get_version + +from isaaclab.managers import EventManager +from isaaclab.scene import InteractiveScene +from isaaclab.sim import SimulationContext +from isaaclab.utils.noise import NoiseModel +from isaaclab.utils.timer import Timer + +from .common import VecEnvObs, VecEnvStepReturn +from .direct_rl_env_cfg import DirectRLEnvCfg +from .ui import ViewportCameraController +from .utils.spaces import sample_space, spec_to_gym_space + + +class DirectRLEnv(gym.Env): + """The superclass for the direct workflow to design environments. + + This class implements the core functionality for reinforcement learning (RL) + environments. It is designed to be used with any RL library. The class is designed + to be used with vectorized environments, i.e., the environment is expected to be run + in parallel with multiple sub-environments. + + While the environment itself is implemented as a vectorized environment, we do not + inherit from :class:`gym.vector.VectorEnv`. This is mainly because the class adds + various methods (for wait and asynchronous updates) which are not required. + Additionally, each RL library typically has its own definition for a vectorized + environment. Thus, to reduce complexity, we directly use the :class:`gym.Env` over + here and leave it up to library-defined wrappers to take care of wrapping this + environment for their agents. + + Note: + For vectorized environments, it is recommended to **only** call the :meth:`reset` + method once before the first call to :meth:`step`, i.e. after the environment is created. + After that, the :meth:`step` function handles the reset of terminated sub-environments. + This is because the simulator does not support resetting individual sub-environments + in a vectorized environment. + + """ + + is_vector_env: ClassVar[bool] = True + """Whether the environment is a vectorized environment.""" + metadata: ClassVar[dict[str, Any]] = { + "render_modes": [None, "human", "rgb_array"], + "isaac_sim_version": get_version(), + } + """Metadata for the environment.""" + + def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs): + """Initialize the environment. + + Args: + cfg: The configuration object for the environment. + render_mode: The render mode for the environment. Defaults to None, which + is similar to ``"human"``. + + Raises: + RuntimeError: If a simulation context already exists. The environment must always create one + since it configures the simulation context and controls the simulation. + """ + # check that the config is valid + cfg.validate() + # store inputs to class + self.cfg = cfg + # store the render mode + self.render_mode = render_mode + # initialize internal variables + self._is_closed = False + + # set the seed for the environment + if self.cfg.seed is not None: + self.cfg.seed = self.seed(self.cfg.seed) + else: + omni.log.warn("Seed not set for the environment. The environment creation may not be deterministic.") + + # create a simulation context to control the simulator + if SimulationContext.instance() is None: + self.sim: SimulationContext = SimulationContext(self.cfg.sim) + else: + raise RuntimeError("Simulation context already exists. Cannot create a new one.") + + # make sure torch is running on the correct device + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + # print useful information + print("[INFO]: Base environment:") + print(f"\tEnvironment device : {self.device}") + print(f"\tEnvironment seed : {self.cfg.seed}") + print(f"\tPhysics step-size : {self.physics_dt}") + print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}") + print(f"\tEnvironment step-size : {self.step_dt}") + + if self.cfg.sim.render_interval < self.cfg.decimation: + msg = ( + f"The render interval ({self.cfg.sim.render_interval}) is smaller than the decimation " + f"({self.cfg.decimation}). Multiple render calls will happen for each environment step." + "If this is not intended, set the render interval to be equal to the decimation." + ) + omni.log.warn(msg) + + # generate scene + with Timer("[INFO]: Time taken for scene creation", "scene_creation"): + self.scene = InteractiveScene(self.cfg.scene) + self._setup_scene() + print("[INFO]: Scene manager: ", self.scene) + + # set up camera viewport controller + # viewport is not available in other rendering modes so the function will throw a warning + # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for + # non-rendering modes. + if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) + else: + self.viewport_camera_controller = None + + # create event manager + # note: this is needed here (rather than after simulation play) to allow USD-related randomization events + # that must happen before the simulation starts. Example: randomizing mesh scale + if self.cfg.events: + self.event_manager = EventManager(self.cfg.events, self) + + # apply USD-related randomization events + if "prestartup" in self.event_manager.available_modes: + self.event_manager.apply(mode="prestartup") + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + # note: when started in extension mode, first call sim.reset_async() and then initialize the managers + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) + + # check if debug visualization is has been implemented by the environment + source_code = inspect.getsource(self._set_debug_vis_impl) + self.has_debug_vis_implementation = "NotImplementedError" not in source_code + self._debug_vis_handle = None + + # extend UI elements + # we need to do this here after all the managers are initialized + # this is because they dictate the sensors and commands right now + if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") + else: + # if no window, then we don't need to store the window + self._window = None + + # allocate dictionary to store metrics + self.extras = {} + + # initialize data and constants + # -- counter for simulation steps + self._sim_step_counter = 0 + # -- counter for curriculum + self.common_step_counter = 0 + # -- init buffers + self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + self.reset_terminated = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self.reset_time_outs = torch.zeros_like(self.reset_terminated) + self.reset_buf = torch.zeros(self.num_envs, dtype=torch.bool, device=self.sim.device) + + # setup the action and observation spaces for Gym + self._configure_gym_env_spaces() + + # setup noise cfg for adding action and observation noise + if self.cfg.action_noise_model: + self._action_noise_model: NoiseModel = self.cfg.action_noise_model.class_type( + self.cfg.action_noise_model, num_envs=self.num_envs, device=self.device + ) + if self.cfg.observation_noise_model: + self._observation_noise_model: NoiseModel = self.cfg.observation_noise_model.class_type( + self.cfg.observation_noise_model, num_envs=self.num_envs, device=self.device + ) + + # perform events at the start of the simulation + if self.cfg.events: + # we print it here to make the logging consistent + print("[INFO] Event Manager: ", self.event_manager) + + if "startup" in self.event_manager.available_modes: + self.event_manager.apply(mode="startup") + + # set the framerate of the gym video recorder wrapper so that the playback speed of the produced + # video matches the simulation + self.metadata["render_fps"] = 1 / self.step_dt + + # print the environment information + print("[INFO]: Completed setting up the environment...") + + def __del__(self): + """Cleanup for the environment.""" + self.close() + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """The number of instances of the environment that are running.""" + return self.scene.num_envs + + @property + def physics_dt(self) -> float: + """The physics time-step (in s). + + This is the lowest time-decimation at which the simulation is happening. + """ + return self.cfg.sim.dt + + @property + def step_dt(self) -> float: + """The environment stepping time-step (in s). + + This is the time-step at which the environment steps forward. + """ + return self.cfg.sim.dt * self.cfg.decimation + + @property + def device(self): + """The device on which the environment is running.""" + return self.sim.device + + @property + def max_episode_length_s(self) -> float: + """Maximum episode length in seconds.""" + return self.cfg.episode_length_s + + @property + def max_episode_length(self): + """The maximum episode length in steps adjusted from s.""" + return math.ceil(self.max_episode_length_s / (self.cfg.sim.dt * self.cfg.decimation)) + + """ + Operations. + """ + + def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[VecEnvObs, dict]: + """Resets all the environments and returns observations. + + This function calls the :meth:`_reset_idx` function to reset all the environments. + However, certain operations, such as procedural terrain generation, that happened during initialization + are not repeated. + + Args: + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + options: Additional information to specify how the environment is reset. Defaults to None. + + Note: + This argument is used for compatibility with Gymnasium environment definition. + + Returns: + A tuple containing the observations and extras. + """ + # set the seed + if seed is not None: + self.seed(seed) + + # reset state of scene + indices = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + self._reset_idx(indices) + + # update articulation kinematics + self.scene.write_data_to_sim() + self.sim.forward() + + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: + self.sim.render() + + if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): + while SimulationManager.assets_loading(): + self.sim.render() + + # return observations + return self._get_observations(), self.extras + + def step(self, action: torch.Tensor) -> VecEnvStepReturn: + """Execute one time-step of the environment's dynamics. + + The environment steps forward at a fixed time-step, while the physics simulation is decimated at a + lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured + independently using the :attr:`DirectRLEnvCfg.decimation` (number of simulation steps per environment step) + and the :attr:`DirectRLEnvCfg.sim.physics_dt` (physics time-step). Based on these parameters, the environment + time-step is computed as the product of the two. + + This function performs the following steps: + + 1. Pre-process the actions before stepping through the physics. + 2. Apply the actions to the simulator and step through the physics in a decimated manner. + 3. Compute the reward and done signals. + 4. Reset environments that have terminated or reached the maximum episode length. + 5. Apply interval events if they are enabled. + 6. Compute observations. + + Args: + action: The actions to apply on the environment. Shape is (num_envs, action_dim). + + Returns: + A tuple containing the observations, rewards, resets (terminated and truncated) and extras. + """ + action = action.to(self.device) + # add action noise + if self.cfg.action_noise_model: + action = self._action_noise_model.apply(action) + + # process actions + self._pre_physics_step(action) + + # check if we need to do rendering within the physics loop + # note: checked here once to avoid multiple checks within the loop + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self._apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # post-step: + # -- update env counters (used for curriculum generation) + self.episode_length_buf += 1 # step in current episode (per env) + self.common_step_counter += 1 # total step (common for all envs) + + self.reset_terminated[:], self.reset_time_outs[:] = self._get_dones() + self.reset_buf = self.reset_terminated | self.reset_time_outs + self.reward_buf = self._get_rewards() + + # -- reset envs that terminated/timed-out and log the episode information + reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(reset_env_ids) > 0: + self._reset_idx(reset_env_ids) + # update articulation kinematics + self.scene.write_data_to_sim() + self.sim.forward() + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: + self.sim.render() + + # post-step: step interval event + if self.cfg.events: + if "interval" in self.event_manager.available_modes: + self.event_manager.apply(mode="interval", dt=self.step_dt) + + # update observations + self.obs_buf = self._get_observations() + + # add observation noise + # note: we apply no noise to the state space (since it is used for critic networks) + if self.cfg.observation_noise_model: + self.obs_buf["policy"] = self._observation_noise_model.apply(self.obs_buf["policy"]) + + # return observations, rewards, resets and extras + return self.obs_buf, self.reward_buf, self.reset_terminated, self.reset_time_outs, self.extras + + @staticmethod + def seed(seed: int = -1) -> int: + """Set the seed for the environment. + + Args: + seed: The seed for random generator. Defaults to -1. + + Returns: + The seed used for random generator. + """ + # set seed for replicator + try: + import omni.replicator.core as rep + + rep.set_global_seed(seed) + except ModuleNotFoundError: + pass + # set seed for torch and other libraries + return torch_utils.set_seed(seed) + + def render(self, recompute: bool = False) -> np.ndarray | None: + """Run rendering without stepping through the physics. + + By convention, if mode is: + + - **human**: Render to the current display and return nothing. Usually for human consumption. + - **rgb_array**: Return an numpy.ndarray with shape (x, y, 3), representing RGB values for an + x-by-y pixel image, suitable for turning into a video. + + Args: + recompute: Whether to force a render even if the simulator has already rendered the scene. + Defaults to False. + + Returns: + The rendered image as a numpy array if mode is "rgb_array". Otherwise, returns None. + + Raises: + RuntimeError: If mode is set to "rgb_data" and simulation render mode does not support it. + In this case, the simulation render mode must be set to ``RenderMode.PARTIAL_RENDERING`` + or ``RenderMode.FULL_RENDERING``. + NotImplementedError: If an unsupported rendering mode is specified. + """ + # run a rendering step of the simulator + # if we have rtx sensors, we do not need to render again sin + if not self.sim.has_rtx_sensors() and not recompute: + self.sim.render() + # decide the rendering mode + if self.render_mode == "human" or self.render_mode is None: + return None + elif self.render_mode == "rgb_array": + # check that if any render could have happened + if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + raise RuntimeError( + f"Cannot render '{self.render_mode}' when the simulation render mode is" + f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." + " If running headless, make sure --enable_cameras is set." + ) + # create the annotator if it does not exist + if not hasattr(self, "_rgb_annotator"): + import omni.replicator.core as rep + + # create render product + self._render_product = rep.create.render_product( + self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution + ) + # create rgb annotator -- used to read data from the render product + self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") + self._rgb_annotator.attach([self._render_product]) + # obtain the rgb data + rgb_data = self._rgb_annotator.get_data() + # convert to numpy array + rgb_data = np.frombuffer(rgb_data, dtype=np.uint8).reshape(*rgb_data.shape) + # return the rgb data + # note: initially the renerer is warming up and returns empty data + if rgb_data.size == 0: + return np.zeros((self.cfg.viewer.resolution[1], self.cfg.viewer.resolution[0], 3), dtype=np.uint8) + else: + return rgb_data[:, :, :3] + else: + raise NotImplementedError( + f"Render mode '{self.render_mode}' is not supported. Please use: {self.metadata['render_modes']}." + ) + + def close(self): + """Cleanup for the environment.""" + if not self._is_closed: + # close entities related to the environment + # note: this is order-sensitive to avoid any dangling references + if self.cfg.events: + del self.event_manager + del self.scene + if self.viewport_camera_controller is not None: + del self.viewport_camera_controller + # clear callbacks and instance + self.sim.clear_all_callbacks() + self.sim.clear_instance() + # destroy the window + if self._window is not None: + self._window = None + # update closing status + self._is_closed = True + + """ + Operations - Debug Visualization. + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Toggles the environment debug visualization. + + Args: + debug_vis: Whether to visualize the environment debug visualization. + + Returns: + Whether the debug visualization was successfully set. False if the environment + does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + """ + Helper functions. + """ + + def _configure_gym_env_spaces(self): + """Configure the action and observation spaces for the Gym environment.""" + # show deprecation message and overwrite configuration + if self.cfg.num_actions is not None: + omni.log.warn("DirectRLEnvCfg.num_actions is deprecated. Use DirectRLEnvCfg.action_space instead.") + if isinstance(self.cfg.action_space, type(MISSING)): + self.cfg.action_space = self.cfg.num_actions + if self.cfg.num_observations is not None: + omni.log.warn( + "DirectRLEnvCfg.num_observations is deprecated. Use DirectRLEnvCfg.observation_space instead." + ) + if isinstance(self.cfg.observation_space, type(MISSING)): + self.cfg.observation_space = self.cfg.num_observations + if self.cfg.num_states is not None: + omni.log.warn("DirectRLEnvCfg.num_states is deprecated. Use DirectRLEnvCfg.state_space instead.") + if isinstance(self.cfg.state_space, type(MISSING)): + self.cfg.state_space = self.cfg.num_states + + # set up spaces + self.single_observation_space = gym.spaces.Dict() + self.single_observation_space["policy"] = spec_to_gym_space(self.cfg.observation_space) + self.single_action_space = spec_to_gym_space(self.cfg.action_space) + + # batch the spaces for vectorized environments + self.observation_space = gym.vector.utils.batch_space(self.single_observation_space["policy"], self.num_envs) + self.action_space = gym.vector.utils.batch_space(self.single_action_space, self.num_envs) + + # optional state space for asymmetric actor-critic architectures + self.state_space = None + if self.cfg.state_space: + self.single_observation_space["critic"] = spec_to_gym_space(self.cfg.state_space) + self.state_space = gym.vector.utils.batch_space(self.single_observation_space["critic"], self.num_envs) + + # instantiate actions (needed for tasks for which the observations computation is dependent on the actions) + self.actions = sample_space(self.single_action_space, self.sim.device, batch_size=self.num_envs, fill_value=0) + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset environments based on specified indices. + + Args: + env_ids: List of environment ids which must be reset + """ + self.scene.reset(env_ids) + + # apply events such as randomization for environments that need a reset + if self.cfg.events: + if "reset" in self.event_manager.available_modes: + env_step_count = self._sim_step_counter // self.cfg.decimation + self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) + + # reset noise models + if self.cfg.action_noise_model: + self._action_noise_model.reset(env_ids) + if self.cfg.observation_noise_model: + self._observation_noise_model.reset(env_ids) + + # reset the episode length buffer + self.episode_length_buf[env_ids] = 0 + + """ + Implementation-specific functions. + """ + + def _setup_scene(self): + """Setup the scene for the environment. + + This function is responsible for creating the scene objects and setting up the scene for the environment. + The scene creation can happen through :class:`isaaclab.scene.InteractiveSceneCfg` or through + directly creating the scene objects and registering them with the scene manager. + + We leave the implementation of this function to the derived classes. If the environment does not require + any explicit scene setup, the function can be left empty. + """ + pass + + @abstractmethod + def _pre_physics_step(self, actions: torch.Tensor): + """Pre-process actions before stepping through the physics. + + This function is responsible for pre-processing the actions before stepping through the physics. + It is called before the physics stepping (which is decimated). + + Args: + actions: The actions to apply on the environment. Shape is (num_envs, action_dim). + """ + raise NotImplementedError(f"Please implement the '_pre_physics_step' method for {self.__class__.__name__}.") + + @abstractmethod + def _apply_action(self): + """Apply actions to the simulator. + + This function is responsible for applying the actions to the simulator. It is called at each + physics time-step. + """ + raise NotImplementedError(f"Please implement the '_apply_action' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_observations(self) -> VecEnvObs: + """Compute and return the observations for the environment. + + Returns: + The observations for the environment. + """ + raise NotImplementedError(f"Please implement the '_get_observations' method for {self.__class__.__name__}.") + + def _get_states(self) -> VecEnvObs | None: + """Compute and return the states for the environment. + + The state-space is used for asymmetric actor-critic architectures. It is configured + using the :attr:`DirectRLEnvCfg.state_space` parameter. + + Returns: + The states for the environment. If the environment does not have a state-space, the function + returns a None. + """ + return None # noqa: R501 + + @abstractmethod + def _get_rewards(self) -> torch.Tensor: + """Compute and return the rewards for the environment. + + Returns: + The rewards for the environment. Shape is (num_envs,). + """ + raise NotImplementedError(f"Please implement the '_get_rewards' method for {self.__class__.__name__}.") + + @abstractmethod + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + """Compute and return the done flags for the environment. + + Returns: + A tuple containing the done flags for termination and time-out. + Shape of individual tensors is (num_envs,). + """ + raise NotImplementedError(f"Please implement the '_get_dones' method for {self.__class__.__name__}.") + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env_cfg.py new file mode 100644 index 00000000000..3cfb4b5035b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/direct_rl_env_cfg.py @@ -0,0 +1,236 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.devices.openxr import XrCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass +from isaaclab.utils.noise import NoiseModelCfg + +from .common import SpaceType, ViewerCfg +from .ui import BaseEnvWindow + + +@configclass +class DirectRLEnvCfg: + """Configuration for an RL environment defined with the direct workflow. + + Please refer to the :class:`isaaclab.envs.direct_rl_env.DirectRLEnv` class for more details. + """ + + # simulation settings + viewer: ViewerCfg = ViewerCfg() + """Viewer configuration. Default is ViewerCfg().""" + + sim: SimulationCfg = SimulationCfg() + """Physics simulation configuration. Default is SimulationCfg().""" + + # ui settings + ui_window_class_type: type | None = BaseEnvWindow + """The class type of the UI window. Default is None. + + If None, then no UI window is created. + + Note: + If you want to make your own UI window, you can create a class that inherits from + from :class:`isaaclab.envs.ui.base_env_window.BaseEnvWindow`. Then, you can set + this attribute to your class type. + """ + + # general settings + seed: int | None = None + """The seed for the random number generator. Defaults to None, in which case the seed is not set. + + Note: + The seed is set at the beginning of the environment initialization. This ensures that the environment + creation is deterministic and behaves similarly across different runs. + """ + + decimation: int = MISSING + """Number of control action updates @ sim dt per policy dt. + + For instance, if the simulation dt is 0.01s and the policy dt is 0.1s, then the decimation is 10. + This means that the control action is updated every 10 simulation steps. + """ + + is_finite_horizon: bool = False + """Whether the learning task is treated as a finite or infinite horizon problem for the agent. + Defaults to False, which means the task is treated as an infinite horizon problem. + + This flag handles the subtleties of finite and infinite horizon tasks: + + * **Finite horizon**: no penalty or bootstrapping value is required by the the agent for + running out of time. However, the environment still needs to terminate the episode after the + time limit is reached. + * **Infinite horizon**: the agent needs to bootstrap the value of the state at the end of the episode. + This is done by sending a time-limit (or truncated) done signal to the agent, which triggers this + bootstrapping calculation. + + If True, then the environment is treated as a finite horizon problem and no time-out (or truncated) done signal + is sent to the agent. If False, then the environment is treated as an infinite horizon problem and a time-out + (or truncated) done signal is sent to the agent. + + Note: + The base :class:`ManagerBasedRLEnv` class does not use this flag directly. It is used by the environment + wrappers to determine what type of done signal to send to the corresponding learning agent. + """ + + episode_length_s: float = MISSING + """Duration of an episode (in seconds). + + Based on the decimation rate and physics time step, the episode length is calculated as: + + .. code-block:: python + + episode_length_steps = ceil(episode_length_s / (decimation_rate * physics_time_step)) + + For example, if the decimation rate is 10, the physics time step is 0.01, and the episode length is 10 seconds, + then the episode length in steps is 100. + """ + + # environment settings + scene: InteractiveSceneCfg = MISSING + """Scene settings. + + Please refer to the :class:`isaaclab.scene.InteractiveSceneCfg` class for more details. + """ + + events: object | None = None + """Event settings. Defaults to None, in which case no events are applied through the event manager. + + Please refer to the :class:`isaaclab.managers.EventManager` class for more details. + """ + + observation_space: SpaceType = MISSING + """Observation space definition. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_observations: int | None = None + """The dimension of the observation space from each environment instance. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectRLEnvCfg.observation_space` instead. + """ + + state_space: SpaceType | None = None + """State space definition. + + This is useful for asymmetric actor-critic and defines the observation space for the critic. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_states: int | None = None + """The dimension of the state-space from each environment instance. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectRLEnvCfg.state_space` instead. + """ + + observation_noise_model: NoiseModelCfg | None = None + """The noise model to apply to the computed observations from the environment. Default is None, which means no noise is added. + + Please refer to the :class:`isaaclab.utils.noise.NoiseModel` class for more details. + """ + + action_space: SpaceType = MISSING + """Action space definition. + + The space can be defined either using Gymnasium :py:mod:`~gymnasium.spaces` (when a more detailed + specification of the space is desired) or basic Python data types (for simplicity). + + .. list-table:: + :header-rows: 1 + + * - Gymnasium space + - Python data type + * - :class:`~gymnasium.spaces.Box` + - Integer or list of integers (e.g.: ``7``, ``[64, 64, 3]``) + * - :class:`~gymnasium.spaces.Discrete` + - Single-element set (e.g.: ``{2}``) + * - :class:`~gymnasium.spaces.MultiDiscrete` + - List of single-element sets (e.g.: ``[{2}, {5}]``) + * - :class:`~gymnasium.spaces.Dict` + - Dictionary (e.g.: ``{"joints": 7, "rgb": [64, 64, 3], "gripper": {2}}``) + * - :class:`~gymnasium.spaces.Tuple` + - Tuple (e.g.: ``(7, [64, 64, 3], {2})``) + """ + + num_actions: int | None = None + """The dimension of the action space for each environment. + + .. warning:: + + This attribute is deprecated. Use :attr:`~isaaclab.envs.DirectRLEnvCfg.action_space` instead. + """ + + action_noise_model: NoiseModelCfg | None = None + """The noise model applied to the actions provided to the environment. Default is None, which means no noise is added. + + Please refer to the :class:`isaaclab.utils.noise.NoiseModel` class for more details. + """ + + rerender_on_reset: bool = False + """Whether a render step is performed again after at least one environment has been reset. + Defaults to False, which means no render step will be performed after reset. + + * When this is False, data collected from sensors after performing reset will be stale and will not reflect the + latest states in simulation caused by the reset. + * When this is True, an extra render step will be performed to update the sensor data + to reflect the latest states from the reset. This comes at a cost of performance as an additional render + step will be performed after each time an environment is reset. + + """ + + wait_for_textures: bool = True + """True to wait for assets to be loaded completely, False otherwise. Defaults to True.""" + + xr: XrCfg | None = None + """Configuration for viewing and interacting with the environment through an XR device.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env.py new file mode 100644 index 00000000000..425d4123e47 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env.py @@ -0,0 +1,502 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import builtins +import torch +from collections.abc import Sequence +from typing import Any + +import isaacsim.core.utils.torch as torch_utils +import omni.log +from isaacsim.core.simulation_manager import SimulationManager + +from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager +from isaaclab.scene import InteractiveScene +from isaaclab.sim import SimulationContext +from isaaclab.ui.widgets import ManagerLiveVisualizer +from isaaclab.utils.timer import Timer + +from .common import VecEnvObs +from .manager_based_env_cfg import ManagerBasedEnvCfg +from .ui import ViewportCameraController + + +class ManagerBasedEnv: + """The base environment encapsulates the simulation scene and the environment managers for the manager-based workflow. + + While a simulation scene or world comprises of different components such as the robots, objects, + and sensors (cameras, lidars, etc.), the environment is a higher level abstraction + that provides an interface for interacting with the simulation. The environment is comprised of + the following components: + + * **Scene**: The scene manager that creates and manages the virtual world in which the robot operates. + This includes defining the robot, static and dynamic objects, sensors, etc. + * **Observation Manager**: The observation manager that generates observations from the current simulation + state and the data gathered from the sensors. These observations may include privileged information + that is not available to the robot in the real world. Additionally, user-defined terms can be added + to process the observations and generate custom observations. For example, using a network to embed + high-dimensional observations into a lower-dimensional space. + * **Action Manager**: The action manager that processes the raw actions sent to the environment and + converts them to low-level commands that are sent to the simulation. It can be configured to accept + raw actions at different levels of abstraction. For example, in case of a robotic arm, the raw actions + can be joint torques, joint positions, or end-effector poses. Similarly for a mobile base, it can be + the joint torques, or the desired velocity of the floating base. + * **Event Manager**: The event manager orchestrates operations triggered based on simulation events. + This includes resetting the scene to a default state, applying random pushes to the robot at different intervals + of time, or randomizing properties such as mass and friction coefficients. This is useful for training + and evaluating the robot in a variety of scenarios. + * **Recorder Manager**: The recorder manager that handles recording data produced during different steps + in the simulation. This includes recording in the beginning and end of a reset and a step. The recorded data + is distinguished per episode, per environment and can be exported through a dataset file handler to a file. + + The environment provides a unified interface for interacting with the simulation. However, it does not + include task-specific quantities such as the reward function, or the termination conditions. These + quantities are often specific to defining Markov Decision Processes (MDPs) while the base environment + is agnostic to the MDP definition. + + The environment steps forward in time at a fixed time-step. The physics simulation is decimated at a + lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured + independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of simulation steps per environment step) + and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step) parameters. Based on these parameters, the + environment time-step is computed as the product of the two. The two time-steps can be obtained by + querying the :attr:`physics_dt` and the :attr:`step_dt` properties respectively. + """ + + def __init__(self, cfg: ManagerBasedEnvCfg): + """Initialize the environment. + + Args: + cfg: The configuration object for the environment. + + Raises: + RuntimeError: If a simulation context already exists. The environment must always create one + since it configures the simulation context and controls the simulation. + """ + # check that the config is valid + cfg.validate() + # store inputs to class + self.cfg = cfg + # initialize internal variables + self._is_closed = False + + # set the seed for the environment + if self.cfg.seed is not None: + self.cfg.seed = self.seed(self.cfg.seed) + else: + omni.log.warn("Seed not set for the environment. The environment creation may not be deterministic.") + + # create a simulation context to control the simulator + if SimulationContext.instance() is None: + # the type-annotation is required to avoid a type-checking error + # since it gets confused with Isaac Sim's SimulationContext class + self.sim: SimulationContext = SimulationContext(self.cfg.sim) + else: + # simulation context should only be created before the environment + # when in extension mode + if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: + raise RuntimeError("Simulation context already exists. Cannot create a new one.") + self.sim: SimulationContext = SimulationContext.instance() + + # make sure torch is running on the correct device + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + # print useful information + print("[INFO]: Base environment:") + print(f"\tEnvironment device : {self.device}") + print(f"\tEnvironment seed : {self.cfg.seed}") + print(f"\tPhysics step-size : {self.physics_dt}") + print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}") + print(f"\tEnvironment step-size : {self.step_dt}") + + if self.cfg.sim.render_interval < self.cfg.decimation: + msg = ( + f"The render interval ({self.cfg.sim.render_interval}) is smaller than the decimation " + f"({self.cfg.decimation}). Multiple render calls will happen for each environment step. " + "If this is not intended, set the render interval to be equal to the decimation." + ) + omni.log.warn(msg) + + # counter for simulation steps + self._sim_step_counter = 0 + + # allocate dictionary to store metrics + self.extras = {} + + # generate scene + with Timer("[INFO]: Time taken for scene creation", "scene_creation"): + self.scene = InteractiveScene(self.cfg.scene) + print("[INFO]: Scene manager: ", self.scene) + + # set up camera viewport controller + # viewport is not available in other rendering modes so the function will throw a warning + # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for + # non-rendering modes. + if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) + else: + self.viewport_camera_controller = None + + # create event manager + # note: this is needed here (rather than after simulation play) to allow USD-related randomization events + # that must happen before the simulation starts. Example: randomizing mesh scale + self.event_manager = EventManager(self.cfg.events, self) + + # apply USD-related randomization events + if "prestartup" in self.event_manager.available_modes: + self.event_manager.apply(mode="prestartup") + + # play the simulator to activate physics handles + # note: this activates the physics simulation view that exposes TensorAPIs + # note: when started in extension mode, first call sim.reset_async() and then initialize the managers + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) + # add timeline event to load managers + self.load_managers() + + # extend UI elements + # we need to do this here after all the managers are initialized + # this is because they dictate the sensors and commands right now + if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + # setup live visualizers + self.setup_manager_visualizers() + self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") + else: + # if no window, then we don't need to store the window + self._window = None + + # initialize observation buffers + self.obs_buf = {} + + def __del__(self): + """Cleanup for the environment.""" + self.close() + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """The number of instances of the environment that are running.""" + return self.scene.num_envs + + @property + def physics_dt(self) -> float: + """The physics time-step (in s). + + This is the lowest time-decimation at which the simulation is happening. + """ + return self.cfg.sim.dt + + @property + def step_dt(self) -> float: + """The environment stepping time-step (in s). + + This is the time-step at which the environment steps forward. + """ + return self.cfg.sim.dt * self.cfg.decimation + + @property + def device(self): + """The device on which the environment is running.""" + return self.sim.device + + """ + Operations - Setup. + """ + + def load_managers(self): + """Load the managers for the environment. + + This function is responsible for creating the various managers (action, observation, + events, etc.) for the environment. Since the managers require access to physics handles, + they can only be created after the simulator is reset (i.e. played for the first time). + + .. note:: + In case of standalone application (when running simulator from Python), the function is called + automatically when the class is initialized. + + However, in case of extension mode, the user must call this function manually after the simulator + is reset. This is because the simulator is only reset when the user calls + :meth:`SimulationContext.reset_async` and it isn't possible to call async functions in the constructor. + + """ + # prepare the managers + # -- event manager (we print it here to make the logging consistent) + print("[INFO] Event Manager: ", self.event_manager) + # -- recorder manager + self.recorder_manager = RecorderManager(self.cfg.recorders, self) + print("[INFO] Recorder Manager: ", self.recorder_manager) + # -- action manager + self.action_manager = ActionManager(self.cfg.actions, self) + print("[INFO] Action Manager: ", self.action_manager) + # -- observation manager + self.observation_manager = ObservationManager(self.cfg.observations, self) + print("[INFO] Observation Manager:", self.observation_manager) + + # perform events at the start of the simulation + # in-case a child implementation creates other managers, the randomization should happen + # when all the other managers are created + if self.__class__ == ManagerBasedEnv and "startup" in self.event_manager.available_modes: + self.event_manager.apply(mode="startup") + + def setup_manager_visualizers(self): + """Creates live visualizers for manager terms.""" + + self.manager_visualizers = { + "action_manager": ManagerLiveVisualizer(manager=self.action_manager), + "observation_manager": ManagerLiveVisualizer(manager=self.observation_manager), + } + + """ + Operations - MDP. + """ + + def reset( + self, seed: int | None = None, env_ids: Sequence[int] | None = None, options: dict[str, Any] | None = None + ) -> tuple[VecEnvObs, dict]: + """Resets the specified environments and returns observations. + + This function calls the :meth:`_reset_idx` function to reset the specified environments. + However, certain operations, such as procedural terrain generation, that happened during initialization + are not repeated. + + Args: + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. + options: Additional information to specify how the environment is reset. Defaults to None. + + Note: + This argument is used for compatibility with Gymnasium environment definition. + + Returns: + A tuple containing the observations and extras. + """ + if env_ids is None: + env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + + # trigger recorder terms for pre-reset calls + self.recorder_manager.record_pre_reset(env_ids) + + # set the seed + if seed is not None: + self.seed(seed) + + # reset state of scene + self._reset_idx(env_ids) + + # update articulation kinematics + self.scene.write_data_to_sim() + self.sim.forward() + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: + self.sim.render() + + # trigger recorder terms for post-reset calls + self.recorder_manager.record_post_reset(env_ids) + + # compute observations + self.obs_buf = self.observation_manager.compute() + + if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): + while SimulationManager.assets_loading(): + self.sim.render() + + # return observations + return self.obs_buf, self.extras + + def reset_to( + self, + state: dict[str, dict[str, dict[str, torch.Tensor]]], + env_ids: Sequence[int] | None, + seed: int | None = None, + is_relative: bool = False, + ): + """Resets specified environments to provided states. + + This function resets the environments to the provided states. The state is a dictionary + containing the state of the scene entities. Please refer to :meth:`InteractiveScene.get_state` + for the format. + + The function is different from the :meth:`reset` function as it resets the environments to specific states, + instead of using the randomization events for resetting the environments. + + Args: + state: The state to reset the specified environments to. Please refer to + :meth:`InteractiveScene.get_state` for the format. + env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + """ + # reset all envs in the scene if env_ids is None + if env_ids is None: + env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + + # trigger recorder terms for pre-reset calls + self.recorder_manager.record_pre_reset(env_ids) + + # set the seed + if seed is not None: + self.seed(seed) + + self._reset_idx(env_ids) + + # set the state + self.scene.reset_to(state, env_ids, is_relative=is_relative) + + # update articulation kinematics + self.sim.forward() + + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: + self.sim.render() + + # trigger recorder terms for post-reset calls + self.recorder_manager.record_post_reset(env_ids) + + # compute observations + self.obs_buf = self.observation_manager.compute() + + # return observations + return self.obs_buf, self.extras + + def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: + """Execute one time-step of the environment's dynamics. + + The environment steps forward at a fixed time-step, while the physics simulation is + decimated at a lower time-step. This is to ensure that the simulation is stable. These two + time-steps can be configured independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of + simulation steps per environment step) and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step). + Based on these parameters, the environment time-step is computed as the product of the two. + + Args: + action: The actions to apply on the environment. Shape is (num_envs, action_dim). + + Returns: + A tuple containing the observations and extras. + """ + # process actions + self.action_manager.process_action(action.to(self.device)) + + self.recorder_manager.record_pre_step() + + # check if we need to do rendering within the physics loop + # note: checked here once to avoid multiple checks within the loop + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self.action_manager.apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # post-step: step interval event + if "interval" in self.event_manager.available_modes: + self.event_manager.apply(mode="interval", dt=self.step_dt) + + # -- compute observations + self.obs_buf = self.observation_manager.compute() + self.recorder_manager.record_post_step() + + # return observations and extras + return self.obs_buf, self.extras + + @staticmethod + def seed(seed: int = -1) -> int: + """Set the seed for the environment. + + Args: + seed: The seed for random generator. Defaults to -1. + + Returns: + The seed used for random generator. + """ + # set seed for replicator + try: + import omni.replicator.core as rep + + rep.set_global_seed(seed) + except ModuleNotFoundError: + pass + # set seed for torch and other libraries + return torch_utils.set_seed(seed) + + def close(self): + """Cleanup for the environment.""" + if not self._is_closed: + # destructor is order-sensitive + del self.viewport_camera_controller + del self.action_manager + del self.observation_manager + del self.event_manager + del self.recorder_manager + del self.scene + # clear callbacks and instance + self.sim.clear_all_callbacks() + self.sim.clear_instance() + # destroy the window + if self._window is not None: + self._window = None + # update closing status + self._is_closed = True + + """ + Helper functions. + """ + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset environments based on specified indices. + + Args: + env_ids: List of environment ids which must be reset + """ + # reset the internal buffers of the scene elements + self.scene.reset(env_ids) + + # apply events such as randomization for environments that need a reset + if "reset" in self.event_manager.available_modes: + env_step_count = self._sim_step_counter // self.cfg.decimation + self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) + + # iterate over all managers and reset them + # this returns a dictionary of information which is stored in the extras + # note: This is order-sensitive! Certain things need be reset before others. + self.extras["log"] = dict() + # -- observation manager + info = self.observation_manager.reset(env_ids) + self.extras["log"].update(info) + # -- action manager + info = self.action_manager.reset(env_ids) + self.extras["log"].update(info) + # -- event manager + info = self.event_manager.reset(env_ids) + self.extras["log"].update(info) + # -- recorder manager + info = self.recorder_manager.reset(env_ids) + self.extras["log"].update(info) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env_cfg.py new file mode 100644 index 00000000000..fdf36862ae8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_env_cfg.py @@ -0,0 +1,128 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base configuration of the environment. + +This module defines the general configuration of the environment. It includes parameters for +configuring the environment instances, viewer settings, and simulation parameters. +""" + +from dataclasses import MISSING + +import isaaclab.envs.mdp as mdp +from isaaclab.devices.openxr import XrCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import RecorderManagerBaseCfg as DefaultEmptyRecorderManagerCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass + +from .common import ViewerCfg +from .ui import BaseEnvWindow + + +@configclass +class DefaultEventManagerCfg: + """Configuration of the default event manager. + + This manager is used to reset the scene to a default state. The default state is specified + by the scene configuration. + """ + + reset_scene_to_default = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + +@configclass +class ManagerBasedEnvCfg: + """Base configuration of the environment.""" + + # simulation settings + viewer: ViewerCfg = ViewerCfg() + """Viewer configuration. Default is ViewerCfg().""" + + sim: SimulationCfg = SimulationCfg() + """Physics simulation configuration. Default is SimulationCfg().""" + + # ui settings + ui_window_class_type: type | None = BaseEnvWindow + """The class type of the UI window. Default is None. + + If None, then no UI window is created. + + Note: + If you want to make your own UI window, you can create a class that inherits from + from :class:`isaaclab.envs.ui.base_env_window.BaseEnvWindow`. Then, you can set + this attribute to your class type. + """ + + # general settings + seed: int | None = None + """The seed for the random number generator. Defaults to None, in which case the seed is not set. + + Note: + The seed is set at the beginning of the environment initialization. This ensures that the environment + creation is deterministic and behaves similarly across different runs. + """ + + decimation: int = MISSING + """Number of control action updates @ sim dt per policy dt. + + For instance, if the simulation dt is 0.01s and the policy dt is 0.1s, then the decimation is 10. + This means that the control action is updated every 10 simulation steps. + """ + + # environment settings + scene: InteractiveSceneCfg = MISSING + """Scene settings. + + Please refer to the :class:`isaaclab.scene.InteractiveSceneCfg` class for more details. + """ + + recorders: object = DefaultEmptyRecorderManagerCfg() + """Recorder settings. Defaults to recording nothing. + + Please refer to the :class:`isaaclab.managers.RecorderManager` class for more details. + """ + + observations: object = MISSING + """Observation space settings. + + Please refer to the :class:`isaaclab.managers.ObservationManager` class for more details. + """ + + actions: object = MISSING + """Action space settings. + + Please refer to the :class:`isaaclab.managers.ActionManager` class for more details. + """ + + events: object = DefaultEventManagerCfg() + """Event settings. Defaults to the basic configuration that resets the scene to its default state. + + Please refer to the :class:`isaaclab.managers.EventManager` class for more details. + """ + + rerender_on_reset: bool = False + """Whether a render step is performed again after at least one environment has been reset. + Defaults to False, which means no render step will be performed after reset. + + * When this is False, data collected from sensors after performing reset will be stale and will not reflect the + latest states in simulation caused by the reset. + * When this is True, an extra render step will be performed to update the sensor data + to reflect the latest states from the reset. This comes at a cost of performance as an additional render + step will be performed after each time an environment is reset. + + """ + + wait_for_textures: bool = True + """True to wait for assets to be loaded completely, False otherwise. Defaults to True.""" + + xr: XrCfg | None = None + """Configuration for viewing and interacting with the environment through an XR device.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env.py new file mode 100644 index 00000000000..bdcddf9aec1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env.py @@ -0,0 +1,398 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# needed to import for allowing type-hinting: np.ndarray | None +from __future__ import annotations + +import gymnasium as gym +import math +import numpy as np +import torch +from collections.abc import Sequence +from typing import Any, ClassVar + +from isaacsim.core.version import get_version + +from isaaclab.managers import CommandManager, CurriculumManager, RewardManager, TerminationManager +from isaaclab.ui.widgets import ManagerLiveVisualizer + +from .common import VecEnvStepReturn +from .manager_based_env import ManagerBasedEnv +from .manager_based_rl_env_cfg import ManagerBasedRLEnvCfg + + +class ManagerBasedRLEnv(ManagerBasedEnv, gym.Env): + """The superclass for the manager-based workflow reinforcement learning-based environments. + + This class inherits from :class:`ManagerBasedEnv` and implements the core functionality for + reinforcement learning-based environments. It is designed to be used with any RL + library. The class is designed to be used with vectorized environments, i.e., the + environment is expected to be run in parallel with multiple sub-environments. The + number of sub-environments is specified using the ``num_envs``. + + Each observation from the environment is a batch of observations for each sub- + environments. The method :meth:`step` is also expected to receive a batch of actions + for each sub-environment. + + While the environment itself is implemented as a vectorized environment, we do not + inherit from :class:`gym.vector.VectorEnv`. This is mainly because the class adds + various methods (for wait and asynchronous updates) which are not required. + Additionally, each RL library typically has its own definition for a vectorized + environment. Thus, to reduce complexity, we directly use the :class:`gym.Env` over + here and leave it up to library-defined wrappers to take care of wrapping this + environment for their agents. + + Note: + For vectorized environments, it is recommended to **only** call the :meth:`reset` + method once before the first call to :meth:`step`, i.e. after the environment is created. + After that, the :meth:`step` function handles the reset of terminated sub-environments. + This is because the simulator does not support resetting individual sub-environments + in a vectorized environment. + + """ + + is_vector_env: ClassVar[bool] = True + """Whether the environment is a vectorized environment.""" + metadata: ClassVar[dict[str, Any]] = { + "render_modes": [None, "human", "rgb_array"], + "isaac_sim_version": get_version(), + } + """Metadata for the environment.""" + + cfg: ManagerBasedRLEnvCfg + """Configuration for the environment.""" + + def __init__(self, cfg: ManagerBasedRLEnvCfg, render_mode: str | None = None, **kwargs): + """Initialize the environment. + + Args: + cfg: The configuration for the environment. + render_mode: The render mode for the environment. Defaults to None, which + is similar to ``"human"``. + """ + # -- counter for curriculum + self.common_step_counter = 0 + + # initialize the episode length buffer BEFORE loading the managers to use it in mdp functions. + self.episode_length_buf = torch.zeros(cfg.scene.num_envs, device=cfg.sim.device, dtype=torch.long) + + # initialize the base class to setup the scene. + super().__init__(cfg=cfg) + # store the render mode + self.render_mode = render_mode + + # initialize data and constants + # -- set the framerate of the gym video recorder wrapper so that the playback speed of the produced video matches the simulation + self.metadata["render_fps"] = 1 / self.step_dt + + print("[INFO]: Completed setting up the environment...") + + """ + Properties. + """ + + @property + def max_episode_length_s(self) -> float: + """Maximum episode length in seconds.""" + return self.cfg.episode_length_s + + @property + def max_episode_length(self) -> int: + """Maximum episode length in environment steps.""" + return math.ceil(self.max_episode_length_s / self.step_dt) + + """ + Operations - Setup. + """ + + def load_managers(self): + # note: this order is important since observation manager needs to know the command and action managers + # and the reward manager needs to know the termination manager + # -- command manager + self.command_manager: CommandManager = CommandManager(self.cfg.commands, self) + print("[INFO] Command Manager: ", self.command_manager) + + # call the parent class to load the managers for observations and actions. + super().load_managers() + + # prepare the managers + # -- termination manager + self.termination_manager = TerminationManager(self.cfg.terminations, self) + print("[INFO] Termination Manager: ", self.termination_manager) + # -- reward manager + self.reward_manager = RewardManager(self.cfg.rewards, self) + print("[INFO] Reward Manager: ", self.reward_manager) + # -- curriculum manager + self.curriculum_manager = CurriculumManager(self.cfg.curriculum, self) + print("[INFO] Curriculum Manager: ", self.curriculum_manager) + + # setup the action and observation spaces for Gym + self._configure_gym_env_spaces() + + # perform events at the start of the simulation + if "startup" in self.event_manager.available_modes: + self.event_manager.apply(mode="startup") + + def setup_manager_visualizers(self): + """Creates live visualizers for manager terms.""" + + self.manager_visualizers = { + "action_manager": ManagerLiveVisualizer(manager=self.action_manager), + "observation_manager": ManagerLiveVisualizer(manager=self.observation_manager), + "command_manager": ManagerLiveVisualizer(manager=self.command_manager), + "termination_manager": ManagerLiveVisualizer(manager=self.termination_manager), + "reward_manager": ManagerLiveVisualizer(manager=self.reward_manager), + "curriculum_manager": ManagerLiveVisualizer(manager=self.curriculum_manager), + } + + """ + Operations - MDP + """ + + def step(self, action: torch.Tensor) -> VecEnvStepReturn: + """Execute one time-step of the environment's dynamics and reset terminated environments. + + Unlike the :class:`ManagerBasedEnv.step` class, the function performs the following operations: + + 1. Process the actions. + 2. Perform physics stepping. + 3. Perform rendering if gui is enabled. + 4. Update the environment counters and compute the rewards and terminations. + 5. Reset the environments that terminated. + 6. Compute the observations. + 7. Return the observations, rewards, resets and extras. + + Args: + action: The actions to apply on the environment. Shape is (num_envs, action_dim). + + Returns: + A tuple containing the observations, rewards, resets (terminated and truncated) and extras. + """ + # process actions + self.action_manager.process_action(action.to(self.device)) + + self.recorder_manager.record_pre_step() + + # check if we need to do rendering within the physics loop + # note: checked here once to avoid multiple checks within the loop + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self.action_manager.apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # post-step: + # -- update env counters (used for curriculum generation) + self.episode_length_buf += 1 # step in current episode (per env) + self.common_step_counter += 1 # total step (common for all envs) + # -- check terminations + self.reset_buf = self.termination_manager.compute() + self.reset_terminated = self.termination_manager.terminated + self.reset_time_outs = self.termination_manager.time_outs + # -- reward computation + self.reward_buf = self.reward_manager.compute(dt=self.step_dt) + + if len(self.recorder_manager.active_terms) > 0: + # update observations for recording if needed + self.obs_buf = self.observation_manager.compute() + self.recorder_manager.record_post_step() + + # -- reset envs that terminated/timed-out and log the episode information + reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(reset_env_ids) > 0: + # trigger recorder terms for pre-reset calls + self.recorder_manager.record_pre_reset(reset_env_ids) + + self._reset_idx(reset_env_ids) + # update articulation kinematics + self.scene.write_data_to_sim() + self.sim.forward() + + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: + self.sim.render() + + # trigger recorder terms for post-reset calls + self.recorder_manager.record_post_reset(reset_env_ids) + + # -- update command + self.command_manager.compute(dt=self.step_dt) + # -- step interval events + if "interval" in self.event_manager.available_modes: + self.event_manager.apply(mode="interval", dt=self.step_dt) + # -- compute observations + # note: done after reset to get the correct observations for reset envs + self.obs_buf = self.observation_manager.compute() + + # return observations, rewards, resets and extras + return self.obs_buf, self.reward_buf, self.reset_terminated, self.reset_time_outs, self.extras + + def render(self, recompute: bool = False) -> np.ndarray | None: + """Run rendering without stepping through the physics. + + By convention, if mode is: + + - **human**: Render to the current display and return nothing. Usually for human consumption. + - **rgb_array**: Return an numpy.ndarray with shape (x, y, 3), representing RGB values for an + x-by-y pixel image, suitable for turning into a video. + + Args: + recompute: Whether to force a render even if the simulator has already rendered the scene. + Defaults to False. + + Returns: + The rendered image as a numpy array if mode is "rgb_array". Otherwise, returns None. + + Raises: + RuntimeError: If mode is set to "rgb_data" and simulation render mode does not support it. + In this case, the simulation render mode must be set to ``RenderMode.PARTIAL_RENDERING`` + or ``RenderMode.FULL_RENDERING``. + NotImplementedError: If an unsupported rendering mode is specified. + """ + # run a rendering step of the simulator + # if we have rtx sensors, we do not need to render again sin + if not self.sim.has_rtx_sensors() and not recompute: + self.sim.render() + # decide the rendering mode + if self.render_mode == "human" or self.render_mode is None: + return None + elif self.render_mode == "rgb_array": + # check that if any render could have happened + if self.sim.render_mode.value < self.sim.RenderMode.PARTIAL_RENDERING.value: + raise RuntimeError( + f"Cannot render '{self.render_mode}' when the simulation render mode is" + f" '{self.sim.render_mode.name}'. Please set the simulation render mode to:" + f"'{self.sim.RenderMode.PARTIAL_RENDERING.name}' or '{self.sim.RenderMode.FULL_RENDERING.name}'." + " If running headless, make sure --enable_cameras is set." + ) + # create the annotator if it does not exist + if not hasattr(self, "_rgb_annotator"): + import omni.replicator.core as rep + + # create render product + self._render_product = rep.create.render_product( + self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution + ) + # create rgb annotator -- used to read data from the render product + self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") + self._rgb_annotator.attach([self._render_product]) + # obtain the rgb data + rgb_data = self._rgb_annotator.get_data() + # convert to numpy array + rgb_data = np.frombuffer(rgb_data, dtype=np.uint8).reshape(*rgb_data.shape) + # return the rgb data + # note: initially the renerer is warming up and returns empty data + if rgb_data.size == 0: + return np.zeros((self.cfg.viewer.resolution[1], self.cfg.viewer.resolution[0], 3), dtype=np.uint8) + else: + return rgb_data[:, :, :3] + else: + raise NotImplementedError( + f"Render mode '{self.render_mode}' is not supported. Please use: {self.metadata['render_modes']}." + ) + + def close(self): + if not self._is_closed: + # destructor is order-sensitive + del self.command_manager + del self.reward_manager + del self.termination_manager + del self.curriculum_manager + # call the parent class to close the environment + super().close() + + """ + Helper functions. + """ + + def _configure_gym_env_spaces(self): + """Configure the action and observation spaces for the Gym environment.""" + # observation space (unbounded since we don't impose any limits) + self.single_observation_space = gym.spaces.Dict() + for group_name, group_term_names in self.observation_manager.active_terms.items(): + # extract quantities about the group + has_concatenated_obs = self.observation_manager.group_obs_concatenate[group_name] + group_dim = self.observation_manager.group_obs_dim[group_name] + # check if group is concatenated or not + # if not concatenated, then we need to add each term separately as a dictionary + if has_concatenated_obs: + self.single_observation_space[group_name] = gym.spaces.Box(low=-np.inf, high=np.inf, shape=group_dim) + else: + self.single_observation_space[group_name] = gym.spaces.Dict({ + term_name: gym.spaces.Box(low=-np.inf, high=np.inf, shape=term_dim) + for term_name, term_dim in zip(group_term_names, group_dim) + }) + # action space (unbounded since we don't impose any limits) + action_dim = sum(self.action_manager.action_term_dim) + self.single_action_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(action_dim,)) + + # batch the spaces for vectorized environments + self.observation_space = gym.vector.utils.batch_space(self.single_observation_space, self.num_envs) + self.action_space = gym.vector.utils.batch_space(self.single_action_space, self.num_envs) + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset environments based on specified indices. + + Args: + env_ids: List of environment ids which must be reset + """ + # update the curriculum for environments that need a reset + self.curriculum_manager.compute(env_ids=env_ids) + # reset the internal buffers of the scene elements + self.scene.reset(env_ids) + # apply events such as randomizations for environments that need a reset + if "reset" in self.event_manager.available_modes: + env_step_count = self._sim_step_counter // self.cfg.decimation + self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) + + # iterate over all managers and reset them + # this returns a dictionary of information which is stored in the extras + # note: This is order-sensitive! Certain things need be reset before others. + self.extras["log"] = dict() + # -- observation manager + info = self.observation_manager.reset(env_ids) + self.extras["log"].update(info) + # -- action manager + info = self.action_manager.reset(env_ids) + self.extras["log"].update(info) + # -- rewards manager + info = self.reward_manager.reset(env_ids) + self.extras["log"].update(info) + # -- curriculum manager + info = self.curriculum_manager.reset(env_ids) + self.extras["log"].update(info) + # -- command manager + info = self.command_manager.reset(env_ids) + self.extras["log"].update(info) + # -- event manager + info = self.event_manager.reset(env_ids) + self.extras["log"].update(info) + # -- termination manager + info = self.termination_manager.reset(env_ids) + self.extras["log"].update(info) + # -- recorder manager + info = self.recorder_manager.reset(env_ids) + self.extras["log"].update(info) + + # reset the episode length buffer + self.episode_length_buf[env_ids] = 0 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env_cfg.py new file mode 100644 index 00000000000..d7fe91c247e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_env_cfg.py @@ -0,0 +1,85 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .manager_based_env_cfg import ManagerBasedEnvCfg +from .ui import ManagerBasedRLEnvWindow + + +@configclass +class ManagerBasedRLEnvCfg(ManagerBasedEnvCfg): + """Configuration for a reinforcement learning environment with the manager-based workflow.""" + + # ui settings + ui_window_class_type: type | None = ManagerBasedRLEnvWindow + + # general settings + is_finite_horizon: bool = False + """Whether the learning task is treated as a finite or infinite horizon problem for the agent. + Defaults to False, which means the task is treated as an infinite horizon problem. + + This flag handles the subtleties of finite and infinite horizon tasks: + + * **Finite horizon**: no penalty or bootstrapping value is required by the the agent for + running out of time. However, the environment still needs to terminate the episode after the + time limit is reached. + * **Infinite horizon**: the agent needs to bootstrap the value of the state at the end of the episode. + This is done by sending a time-limit (or truncated) done signal to the agent, which triggers this + bootstrapping calculation. + + If True, then the environment is treated as a finite horizon problem and no time-out (or truncated) done signal + is sent to the agent. If False, then the environment is treated as an infinite horizon problem and a time-out + (or truncated) done signal is sent to the agent. + + Note: + The base :class:`ManagerBasedRLEnv` class does not use this flag directly. It is used by the environment + wrappers to determine what type of done signal to send to the corresponding learning agent. + """ + + episode_length_s: float = MISSING + """Duration of an episode (in seconds). + + Based on the decimation rate and physics time step, the episode length is calculated as: + + .. code-block:: python + + episode_length_steps = ceil(episode_length_s / (decimation_rate * physics_time_step)) + + For example, if the decimation rate is 10, the physics time step is 0.01, and the episode length is 10 seconds, + then the episode length in steps is 100. + """ + + # environment settings + rewards: object = MISSING + """Reward settings. + + Please refer to the :class:`isaaclab.managers.RewardManager` class for more details. + """ + + terminations: object = MISSING + """Termination settings. + + Please refer to the :class:`isaaclab.managers.TerminationManager` class for more details. + """ + + curriculum: object | None = None + """Curriculum settings. Defaults to None, in which case no curriculum is applied. + + Please refer to the :class:`isaaclab.managers.CurriculumManager` class for more details. + """ + + commands: object | None = None + """Command settings. Defaults to None, in which case no commands are generated. + + Please refer to the :class:`isaaclab.managers.CommandManager` class for more details. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_mimic_env.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_mimic_env.py new file mode 100644 index 00000000000..e643178c4e1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/manager_based_rl_mimic_env.py @@ -0,0 +1,146 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLEnv + + +class ManagerBasedRLMimicEnv(ManagerBasedRLEnv): + """The superclass for the Isaac Lab Mimic environments. + + This class inherits from :class:`ManagerBasedRLEnv` and provides a template for the functions that + need to be defined to run the Isaac Lab Mimic data generation workflow. The Isaac Lab data generation + pipeline, inspired by the MimicGen system, enables the generation of new datasets based on a few human + collected demonstrations. MimicGen is a novel approach designed to automatically synthesize large-scale, + rich datasets from a sparse set of human demonstrations by adapting them to new contexts. It manages to + replicate the benefits of large datasets while reducing the immense time and effort usually required to + gather extensive human demonstrations. + + The MimicGen system works by parsing demonstrations into object-centric segments. It then adapts + these segments to new scenes by transforming each segment according to the new scene’s context, stitching + them into a coherent trajectory for a robotic end-effector to execute. This approach allows learners to train + proficient agents through imitation learning on diverse configurations of scenes, object instances, etc. + + Key Features: + - Efficient Dataset Generation: Utilizes a small set of human demos to produce large scale demonstrations. + - Broad Applicability: Capable of supporting tasks that require a range of manipulation skills, such as + pick-and-place and interacting with articulated objects. + - Dataset Versatility: The synthetic data retains a quality that compares favorably with additional human demos. + """ + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + raise NotImplementedError + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + action_noise_dict: Noise to add to the action. If None, no noise is added. + env_id: Environment index to compute the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + raise NotImplementedError + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + raise NotImplementedError + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + raise NotImplementedError + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Gets the pose of each object relevant to Isaac Lab Mimic data generation in the current scene. + + Args: + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A dictionary that maps object names to object pose matrix (4x4 torch.Tensor) + """ + if env_ids is None: + env_ids = slice(None) + + rigid_object_states = self.scene.get_state(is_relative=True)["rigid_object"] + object_pose_matrix = dict() + for obj_name, obj_state in rigid_object_states.items(): + object_pose_matrix[obj_name] = PoseUtils.make_pose( + obj_state["root_pose"][env_ids, :3], PoseUtils.matrix_from_quat(obj_state["root_pose"][env_ids, 3:7]) + ) + return object_pose_matrix + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask term signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask term signal annotation. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + raise NotImplementedError + + def serialize(self): + """ + Save all information needed to re-instantiate this environment in a dictionary. + This is the same as @env_meta - environment metadata stored in hdf5 datasets, + and used in utils/env_utils.py. + """ + return dict(env_name=self.spec.id, type=2, env_kwargs=dict()) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/__init__.py new file mode 100644 index 00000000000..47c169b9b22 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/__init__.py @@ -0,0 +1,30 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with implementation of manager terms. + +The functions can be provided to different managers that are responsible for the +different aspects of the MDP. These include the observation, reward, termination, +actions, events and curriculum managers. + +The terms are defined under the ``envs`` module because they are used to define +the environment. However, they are not part of the environment directly, but +are used to define the environment through their managers. + +""" + +from .actions import * # noqa: F401, F403 +from .commands import * # noqa: F401, F403 +from .curriculums import * # noqa: F401, F403 +from .events import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .recorders import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/__init__.py new file mode 100644 index 00000000000..b0bb3d110ef --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Various action terms that can be used in the environment.""" + +from .actions_cfg import * +from .binary_joint_actions import * +from .joint_actions import * +from .joint_actions_to_limits import * +from .non_holonomic_actions import * diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/actions_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/actions_cfg.py new file mode 100644 index 00000000000..81dd4840d16 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/actions_cfg.py @@ -0,0 +1,317 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.controllers import DifferentialIKControllerCfg, OperationalSpaceControllerCfg +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from . import binary_joint_actions, joint_actions, joint_actions_to_limits, non_holonomic_actions, task_space_actions + +## +# Joint actions. +## + + +@configclass +class JointActionCfg(ActionTermCfg): + """Configuration for the base joint action term. + + See :class:`JointAction` for more details. + """ + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + scale: float | dict[str, float] = 1.0 + """Scale factor for the action (float or dict of regex expressions). Defaults to 1.0.""" + offset: float | dict[str, float] = 0.0 + """Offset factor for the action (float or dict of regex expressions). Defaults to 0.0.""" + preserve_order: bool = False + """Whether to preserve the order of the joint names in the action output. Defaults to False.""" + + +@configclass +class JointPositionActionCfg(JointActionCfg): + """Configuration for the joint position action term. + + See :class:`JointPositionAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions.JointPositionAction + + use_default_offset: bool = True + """Whether to use default joint positions configured in the articulation asset as offset. + Defaults to True. + + If True, this flag results in overwriting the values of :attr:`offset` to the default joint positions + from the articulation asset. + """ + + +@configclass +class RelativeJointPositionActionCfg(JointActionCfg): + """Configuration for the relative joint position action term. + + See :class:`RelativeJointPositionAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions.RelativeJointPositionAction + + use_zero_offset: bool = True + """Whether to ignore the offset defined in articulation asset. Defaults to True. + + If True, this flag results in overwriting the values of :attr:`offset` to zero. + """ + + +@configclass +class JointVelocityActionCfg(JointActionCfg): + """Configuration for the joint velocity action term. + + See :class:`JointVelocityAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions.JointVelocityAction + + use_default_offset: bool = True + """Whether to use default joint velocities configured in the articulation asset as offset. + Defaults to True. + + This overrides the settings from :attr:`offset` if set to True. + """ + + +@configclass +class JointEffortActionCfg(JointActionCfg): + """Configuration for the joint effort action term. + + See :class:`JointEffortAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions.JointEffortAction + + +## +# Joint actions rescaled to limits. +## + + +@configclass +class JointPositionToLimitsActionCfg(ActionTermCfg): + """Configuration for the bounded joint position action term. + + See :class:`JointPositionToLimitsAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions_to_limits.JointPositionToLimitsAction + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + + scale: float | dict[str, float] = 1.0 + """Scale factor for the action (float or dict of regex expressions). Defaults to 1.0.""" + + rescale_to_limits: bool = True + """Whether to rescale the action to the joint limits. Defaults to True. + + If True, the input actions are rescaled to the joint limits, i.e., the action value in + the range [-1, 1] corresponds to the joint lower and upper limits respectively. + + Note: + This operation is performed after applying the scale factor. + """ + + +@configclass +class EMAJointPositionToLimitsActionCfg(JointPositionToLimitsActionCfg): + """Configuration for the exponential moving average (EMA) joint position action term. + + See :class:`EMAJointPositionToLimitsAction` for more details. + """ + + class_type: type[ActionTerm] = joint_actions_to_limits.EMAJointPositionToLimitsAction + + alpha: float | dict[str, float] = 1.0 + """The weight for the moving average (float or dict of regex expressions). Defaults to 1.0. + + If set to 1.0, the processed action is applied directly without any moving average window. + """ + + +## +# Gripper actions. +## + + +@configclass +class BinaryJointActionCfg(ActionTermCfg): + """Configuration for the base binary joint action term. + + See :class:`BinaryJointAction` for more details. + """ + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + open_command_expr: dict[str, float] = MISSING + """The joint command to move to *open* configuration.""" + close_command_expr: dict[str, float] = MISSING + """The joint command to move to *close* configuration.""" + + +@configclass +class BinaryJointPositionActionCfg(BinaryJointActionCfg): + """Configuration for the binary joint position action term. + + See :class:`BinaryJointPositionAction` for more details. + """ + + class_type: type[ActionTerm] = binary_joint_actions.BinaryJointPositionAction + + +@configclass +class BinaryJointVelocityActionCfg(BinaryJointActionCfg): + """Configuration for the binary joint velocity action term. + + See :class:`BinaryJointVelocityAction` for more details. + """ + + class_type: type[ActionTerm] = binary_joint_actions.BinaryJointVelocityAction + + +## +# Non-holonomic actions. +## + + +@configclass +class NonHolonomicActionCfg(ActionTermCfg): + """Configuration for the non-holonomic action term with dummy joints at the base. + + See :class:`NonHolonomicAction` for more details. + """ + + class_type: type[ActionTerm] = non_holonomic_actions.NonHolonomicAction + + body_name: str = MISSING + """Name of the body which has the dummy mechanism connected to.""" + x_joint_name: str = MISSING + """The dummy joint name in the x direction.""" + y_joint_name: str = MISSING + """The dummy joint name in the y direction.""" + yaw_joint_name: str = MISSING + """The dummy joint name in the yaw direction.""" + scale: tuple[float, float] = (1.0, 1.0) + """Scale factor for the action. Defaults to (1.0, 1.0).""" + offset: tuple[float, float] = (0.0, 0.0) + """Offset factor for the action. Defaults to (0.0, 0.0).""" + + +## +# Task-space Actions. +## + + +@configclass +class DifferentialInverseKinematicsActionCfg(ActionTermCfg): + """Configuration for inverse differential kinematics action term. + + See :class:`DifferentialInverseKinematicsAction` for more details. + """ + + @configclass + class OffsetCfg: + """The offset pose from parent frame to child frame. + + On many robots, end-effector frames are fictitious frames that do not have a corresponding + rigid body. In such cases, it is easier to define this transform w.r.t. their parent rigid body. + For instance, for the Franka Emika arm, the end-effector is defined at an offset to the the + "panda_hand" frame. + """ + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + class_type: type[ActionTerm] = task_space_actions.DifferentialInverseKinematicsAction + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + body_name: str = MISSING + """Name of the body or frame for which IK is performed.""" + body_offset: OffsetCfg | None = None + """Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied.""" + scale: float | tuple[float, ...] = 1.0 + """Scale factor for the action. Defaults to 1.0.""" + controller: DifferentialIKControllerCfg = MISSING + """The configuration for the differential IK controller.""" + + +@configclass +class OperationalSpaceControllerActionCfg(ActionTermCfg): + """Configuration for operational space controller action term. + + See :class:`OperationalSpaceControllerAction` for more details. + """ + + @configclass + class OffsetCfg: + """The offset pose from parent frame to child frame. + + On many robots, end-effector frames are fictitious frames that do not have a corresponding + rigid body. In such cases, it is easier to define this transform w.r.t. their parent rigid body. + For instance, for the Franka Emika arm, the end-effector is defined at an offset to the the + "panda_hand" frame. + """ + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + class_type: type[ActionTerm] = task_space_actions.OperationalSpaceControllerAction + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + + body_name: str = MISSING + """Name of the body or frame for which motion/force control is performed.""" + + body_offset: OffsetCfg | None = None + """Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied.""" + + task_frame_rel_path: str = None + """The path of a ``RigidObject``, relative to the sub-environment, representing task frame. Defaults to None.""" + + controller_cfg: OperationalSpaceControllerCfg = MISSING + """The configuration for the operational space controller.""" + + position_scale: float = 1.0 + """Scale factor for the position targets. Defaults to 1.0.""" + + orientation_scale: float = 1.0 + """Scale factor for the orientation (quad for ``pose_abs`` or axis-angle for ``pose_rel``). Defaults to 1.0.""" + + wrench_scale: float = 1.0 + """Scale factor for the wrench targets. Defaults to 1.0.""" + + stiffness_scale: float = 1.0 + """Scale factor for the stiffness commands. Defaults to 1.0.""" + + damping_ratio_scale: float = 1.0 + """Scale factor for the damping ratio commands. Defaults to 1.0.""" + + nullspace_joint_pos_target: str = "none" + """The joint targets for the null-space control: ``"none"``, ``"zero"``, ``"default"``, ``"center"``. + + Note: Functional only when ``nullspace_control`` is set to ``"position"`` within the + ``OperationalSpaceControllerCfg``. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/binary_joint_actions.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/binary_joint_actions.py new file mode 100644 index 00000000000..60b3411f7a1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/binary_joint_actions.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + + +class BinaryJointAction(ActionTerm): + """Base class for binary joint actions. + + This action term maps a binary action to the *open* or *close* joint configurations. These configurations are + specified through the :class:`BinaryJointActionCfg` object. If the input action is a float vector, the action + is considered binary based on the sign of the action values. + + Based on above, we follow the following convention for the binary action: + + 1. Open action: 1 (bool) or positive values (float). + 2. Close action: 0 (bool) or negative values (float). + + The action term can mostly be used for gripper actions, where the gripper is either open or closed. This + helps in devising a mimicking mechanism for the gripper, since in simulation it is often not possible to + add such constraints to the gripper. + """ + + cfg: actions_cfg.BinaryJointActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.BinaryJointActionCfg, env: ManagerBasedEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_joints = len(self._joint_ids) + # log the resolved joint names for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, 1, device=self.device) + self._processed_actions = torch.zeros(self.num_envs, self._num_joints, device=self.device) + + # parse open command + self._open_command = torch.zeros(self._num_joints, device=self.device) + index_list, name_list, value_list = string_utils.resolve_matching_names_values( + self.cfg.open_command_expr, self._joint_names + ) + if len(index_list) != self._num_joints: + raise ValueError( + f"Could not resolve all joints for the action term. Missing: {set(self._joint_names) - set(name_list)}" + ) + self._open_command[index_list] = torch.tensor(value_list, device=self.device) + + # parse close command + self._close_command = torch.zeros_like(self._open_command) + index_list, name_list, value_list = string_utils.resolve_matching_names_values( + self.cfg.close_command_expr, self._joint_names + ) + if len(index_list) != self._num_joints: + raise ValueError( + f"Could not resolve all joints for the action term. Missing: {set(self._joint_names) - set(name_list)}" + ) + self._close_command[index_list] = torch.tensor(value_list, device=self.device) + + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return 1 + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + # compute the binary mask + if actions.dtype == torch.bool: + # true: close, false: open + binary_mask = actions == 0 + else: + # true: close, false: open + binary_mask = actions < 0 + # compute the command + self._processed_actions = torch.where(binary_mask, self._close_command, self._open_command) + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + + +class BinaryJointPositionAction(BinaryJointAction): + """Binary joint action that sets the binary action into joint position targets.""" + + cfg: actions_cfg.BinaryJointPositionActionCfg + """The configuration of the action term.""" + + def apply_actions(self): + self._asset.set_joint_position_target(self._processed_actions, joint_ids=self._joint_ids) + + +class BinaryJointVelocityAction(BinaryJointAction): + """Binary joint action that sets the binary action into joint velocity targets.""" + + cfg: actions_cfg.BinaryJointVelocityActionCfg + """The configuration of the action term.""" + + def apply_actions(self): + self._asset.set_joint_velocity_target(self._processed_actions, joint_ids=self._joint_ids) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions.py new file mode 100644 index 00000000000..96144aba6f1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions.py @@ -0,0 +1,230 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + + +class JointAction(ActionTerm): + r"""Base class for joint actions. + + This action term performs pre-processing of the raw actions using affine transformations (scale and offset). + These transformations can be configured to be applied to a subset of the articulation's joints. + + Mathematically, the action term is defined as: + + .. math:: + + \text{action} = \text{offset} + \text{scaling} \times \text{input action} + + where :math:`\text{action}` is the action that is sent to the articulation's actuated joints, :math:`\text{offset}` + is the offset applied to the input action, :math:`\text{scaling}` is the scaling applied to the input + action, and :math:`\text{input action}` is the input action from the user. + + Based on above, this kind of action transformation ensures that the input and output actions are in the same + units and dimensions. The child classes of this action term can then map the output action to a specific + desired command of the articulation's joints (e.g. position, velocity, etc.). + """ + + cfg: actions_cfg.JointActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor | float + """The scaling factor applied to the input action.""" + _offset: torch.Tensor | float + """The offset applied to the input action.""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.JointActionCfg, env: ManagerBasedEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints( + self.cfg.joint_names, preserve_order=self.cfg.preserve_order + ) + self._num_joints = len(self._joint_ids) + # log the resolved joint names for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + + # Avoid indexing across all joints for efficiency + if self._num_joints == self._asset.num_joints and not self.cfg.preserve_order: + self._joint_ids = slice(None) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # parse scale + if isinstance(cfg.scale, (float, int)): + self._scale = float(cfg.scale) + elif isinstance(cfg.scale, dict): + self._scale = torch.ones(self.num_envs, self.action_dim, device=self.device) + # resolve the dictionary config + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.scale, self._joint_names) + self._scale[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported scale type: {type(cfg.scale)}. Supported types are float and dict.") + # parse offset + if isinstance(cfg.offset, (float, int)): + self._offset = float(cfg.offset) + elif isinstance(cfg.offset, dict): + self._offset = torch.zeros_like(self._raw_actions) + # resolve the dictionary config + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.offset, self._joint_names) + self._offset[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported offset type: {type(cfg.offset)}. Supported types are float and dict.") + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return self._num_joints + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + # apply the affine transformations + self._processed_actions = self._raw_actions * self._scale + self._offset + # clip actions + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + + +class JointPositionAction(JointAction): + """Joint action term that applies the processed actions to the articulation's joints as position commands.""" + + cfg: actions_cfg.JointPositionActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: actions_cfg.JointPositionActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + # use default joint positions as offset + if cfg.use_default_offset: + self._offset = self._asset.data.default_joint_pos[:, self._joint_ids].clone() + + def apply_actions(self): + # set position targets + self._asset.set_joint_position_target(self.processed_actions, joint_ids=self._joint_ids) + + +class RelativeJointPositionAction(JointAction): + r"""Joint action term that applies the processed actions to the articulation's joints as relative position commands. + + Unlike :class:`JointPositionAction`, this action term applies the processed actions as relative position commands. + This means that the processed actions are added to the current joint positions of the articulation's joints + before being sent as position commands. + + This means that the action applied at every step is: + + .. math:: + + \text{applied action} = \text{current joint positions} + \text{processed actions} + + where :math:`\text{current joint positions}` are the current joint positions of the articulation's joints. + """ + + cfg: actions_cfg.RelativeJointPositionActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: actions_cfg.RelativeJointPositionActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + # use zero offset for relative position + if cfg.use_zero_offset: + self._offset = 0.0 + + def apply_actions(self): + # add current joint positions to the processed actions + current_actions = self.processed_actions + self._asset.data.joint_pos[:, self._joint_ids] + # set position targets + self._asset.set_joint_position_target(current_actions, joint_ids=self._joint_ids) + + +class JointVelocityAction(JointAction): + """Joint action term that applies the processed actions to the articulation's joints as velocity commands.""" + + cfg: actions_cfg.JointVelocityActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: actions_cfg.JointVelocityActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + # use default joint velocity as offset + if cfg.use_default_offset: + self._offset = self._asset.data.default_joint_vel[:, self._joint_ids].clone() + + def apply_actions(self): + # set joint velocity targets + self._asset.set_joint_velocity_target(self.processed_actions, joint_ids=self._joint_ids) + + +class JointEffortAction(JointAction): + """Joint action term that applies the processed actions to the articulation's joints as effort commands.""" + + cfg: actions_cfg.JointEffortActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: actions_cfg.JointEffortActionCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + + def apply_actions(self): + # set joint effort targets + self._asset.set_joint_effort_target(self.processed_actions, joint_ids=self._joint_ids) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions_to_limits.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions_to_limits.py new file mode 100644 index 00000000000..f1644983d67 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/joint_actions_to_limits.py @@ -0,0 +1,226 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + + +class JointPositionToLimitsAction(ActionTerm): + """Joint position action term that scales the input actions to the joint limits and applies them to the + articulation's joints. + + This class is similar to the :class:`JointPositionAction` class. However, it performs additional + re-scaling of input actions to the actuator joint position limits. + + While processing the actions, it performs the following operations: + + 1. Apply scaling to the raw actions based on :attr:`actions_cfg.JointPositionToLimitsActionCfg.scale`. + 2. Clip the scaled actions to the range [-1, 1] and re-scale them to the joint limits if + :attr:`actions_cfg.JointPositionToLimitsActionCfg.rescale_to_limits` is set to True. + + The processed actions are then sent as position commands to the articulation's joints. + """ + + cfg: actions_cfg.JointPositionToLimitsActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor | float + """The scaling factor applied to the input action.""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.JointPositionToLimitsActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_joints = len(self._joint_ids) + # log the resolved joint names for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + + # Avoid indexing across all joints for efficiency + if self._num_joints == self._asset.num_joints: + self._joint_ids = slice(None) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # parse scale + if isinstance(cfg.scale, (float, int)): + self._scale = float(cfg.scale) + elif isinstance(cfg.scale, dict): + self._scale = torch.ones(self.num_envs, self.action_dim, device=self.device) + # resolve the dictionary config + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.scale, self._joint_names) + self._scale[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported scale type: {type(cfg.scale)}. Supported types are float and dict.") + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return self._num_joints + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + # apply affine transformations + self._processed_actions = self._raw_actions * self._scale + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + # rescale the position targets if configured + # this is useful when the input actions are in the range [-1, 1] + if self.cfg.rescale_to_limits: + # clip to [-1, 1] + actions = self._processed_actions.clamp(-1.0, 1.0) + # rescale within the joint limits + actions = math_utils.unscale_transform( + actions, + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, 0], + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, 1], + ) + self._processed_actions[:] = actions[:] + + def apply_actions(self): + # set position targets + self._asset.set_joint_position_target(self.processed_actions, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + + +class EMAJointPositionToLimitsAction(JointPositionToLimitsAction): + r"""Joint action term that applies exponential moving average (EMA) over the processed actions as the + articulation's joints position commands. + + Exponential moving average (EMA) is a type of moving average that gives more weight to the most recent data points. + This action term applies the processed actions as moving average position action commands. + The moving average is computed as: + + .. math:: + + \text{applied action} = \alpha \times \text{processed actions} + (1 - \alpha) \times \text{previous applied action} + + where :math:`\alpha` is the weight for the moving average, :math:`\text{processed actions}` are the + processed actions, and :math:`\text{previous action}` is the previous action that was applied to the articulation's + joints. + + In the trivial case where the weight is 1.0, the action term behaves exactly like + the :class:`JointPositionToLimitsAction` class. + + On reset, the previous action is initialized to the current joint positions of the articulation's joints. + """ + + cfg: actions_cfg.EMAJointPositionToLimitsActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: actions_cfg.EMAJointPositionToLimitsActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + # parse and save the moving average weight + if isinstance(cfg.alpha, float): + # check that the weight is in the valid range + if not 0.0 <= cfg.alpha <= 1.0: + raise ValueError(f"Moving average weight must be in the range [0, 1]. Got {cfg.alpha}.") + self._alpha = cfg.alpha + elif isinstance(cfg.alpha, dict): + self._alpha = torch.ones((env.num_envs, self.action_dim), device=self.device) + # resolve the dictionary config + index_list, names_list, value_list = string_utils.resolve_matching_names_values( + cfg.alpha, self._joint_names + ) + # check that the weights are in the valid range + for name, value in zip(names_list, value_list): + if not 0.0 <= value <= 1.0: + raise ValueError( + f"Moving average weight must be in the range [0, 1]. Got {value} for joint {name}." + ) + self._alpha[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError( + f"Unsupported moving average weight type: {type(cfg.alpha)}. Supported types are float and dict." + ) + + # initialize the previous targets + self._prev_applied_actions = torch.zeros_like(self.processed_actions) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + # check if specific environment ids are provided + if env_ids is None: + env_ids = slice(None) + else: + env_ids = env_ids[:, None] + super().reset(env_ids) + # reset history to current joint positions + self._prev_applied_actions[env_ids, :] = self._asset.data.joint_pos[env_ids, self._joint_ids] + + def process_actions(self, actions: torch.Tensor): + # apply affine transformations + super().process_actions(actions) + # set position targets as moving average + ema_actions = self._alpha * self._processed_actions + ema_actions += (1.0 - self._alpha) * self._prev_applied_actions + # clamp the targets + self._processed_actions[:] = torch.clamp( + ema_actions, + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, 0], + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, 1], + ) + # update previous targets + self._prev_applied_actions[:] = self._processed_actions[:] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/non_holonomic_actions.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/non_holonomic_actions.py new file mode 100644 index 00000000000..cc9087bf53d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/non_holonomic_actions.py @@ -0,0 +1,168 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.utils.math import euler_xyz_from_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + + +class NonHolonomicAction(ActionTerm): + r"""Non-holonomic action that maps a two dimensional action to the velocity of the robot in + the x, y and yaw directions. + + This action term helps model a skid-steer robot base. The action is a 2D vector which comprises of the + forward velocity :math:`v_{B,x}` and the turning rate :\omega_{B,z}: in the base frame. Using the current + base orientation, the commands are transformed into dummy joint velocity targets as: + + .. math:: + + \dot{q}_{0, des} &= v_{B,x} \cos(\theta) \\ + \dot{q}_{1, des} &= v_{B,x} \sin(\theta) \\ + \dot{q}_{2, des} &= \omega_{B,z} + + where :math:`\theta` is the yaw of the 2-D base. Since the base is simulated as a dummy joint, the yaw is directly + the value of the revolute joint along z, i.e., :math:`q_2 = \theta`. + + .. note:: + The current implementation assumes that the base is simulated with three dummy joints (prismatic joints along x + and y, and revolute joint along z). This is because it is easier to consider the mobile base as a floating link + controlled by three dummy joints, in comparison to simulating wheels which is at times is tricky because of + friction settings. + + However, the action term can be extended to support other base configurations as well. + + .. tip:: + For velocity control of the base with dummy mechanism, we recommend setting high damping gains to the joints. + This ensures that the base remains unperturbed from external disturbances, such as an arm mounted on the base. + """ + + cfg: actions_cfg.NonHolonomicActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor + """The scaling factor applied to the input action. Shape is (1, 2).""" + _offset: torch.Tensor + """The offset applied to the input action. Shape is (1, 2).""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.NonHolonomicActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + # parse the joint information + # -- x joint + x_joint_id, x_joint_name = self._asset.find_joints(self.cfg.x_joint_name) + if len(x_joint_id) != 1: + raise ValueError( + f"Expected a single joint match for the x joint name: {self.cfg.x_joint_name}, got {len(x_joint_id)}" + ) + # -- y joint + y_joint_id, y_joint_name = self._asset.find_joints(self.cfg.y_joint_name) + if len(y_joint_id) != 1: + raise ValueError(f"Found more than one joint match for the y joint name: {self.cfg.y_joint_name}") + # -- yaw joint + yaw_joint_id, yaw_joint_name = self._asset.find_joints(self.cfg.yaw_joint_name) + if len(yaw_joint_id) != 1: + raise ValueError(f"Found more than one joint match for the yaw joint name: {self.cfg.yaw_joint_name}") + # parse the body index + self._body_idx, self._body_name = self._asset.find_bodies(self.cfg.body_name) + if len(self._body_idx) != 1: + raise ValueError(f"Found more than one body match for the body name: {self.cfg.body_name}") + + # process into a list of joint ids + self._joint_ids = [x_joint_id[0], y_joint_id[0], yaw_joint_id[0]] + self._joint_names = [x_joint_name[0], y_joint_name[0], yaw_joint_name[0]] + # log info for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + omni.log.info( + f"Resolved body name for the action term {self.__class__.__name__}: {self._body_name} [{self._body_idx}]" + ) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + self._joint_vel_command = torch.zeros(self.num_envs, 3, device=self.device) + + # save the scale and offset as tensors + self._scale = torch.tensor(self.cfg.scale, device=self.device).unsqueeze(0) + self._offset = torch.tensor(self.cfg.offset, device=self.device).unsqueeze(0) + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return 2 + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + """ + Operations. + """ + + def process_actions(self, actions): + # store the raw actions + self._raw_actions[:] = actions + self._processed_actions = self.raw_actions * self._scale + self._offset + # clip actions + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def apply_actions(self): + # obtain current heading + quat_w = self._asset.data.body_quat_w[:, self._body_idx].view(self.num_envs, 4) + yaw_w = euler_xyz_from_quat(quat_w)[2] + # compute joint velocities targets + self._joint_vel_command[:, 0] = torch.cos(yaw_w) * self.processed_actions[:, 0] # x + self._joint_vel_command[:, 1] = torch.sin(yaw_w) * self.processed_actions[:, 0] # y + self._joint_vel_command[:, 2] = self.processed_actions[:, 1] # yaw + # set the joint velocity targets + self._asset.set_joint_velocity_target(self._joint_vel_command, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_actions_cfg.py new file mode 100644 index 00000000000..2c2dabd9957 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from . import pink_task_space_actions + + +@configclass +class PinkInverseKinematicsActionCfg(ActionTermCfg): + """Configuration for Pink inverse kinematics action term. + + This configuration is used to define settings for the Pink inverse kinematics action term, + which is a inverse kinematics framework. + """ + + class_type: type[ActionTerm] = pink_task_space_actions.PinkInverseKinematicsAction + """Specifies the action term class type for Pink inverse kinematics action.""" + + pink_controlled_joint_names: list[str] = MISSING + """List of joint names or regular expression patterns that specify the joints controlled by pink IK.""" + + ik_urdf_fixed_joint_names: list[str] = MISSING + """List of joint names that specify the joints to be locked in URDF.""" + + hand_joint_names: list[str] = MISSING + """List of joint names or regular expression patterns that specify the joints controlled by hand retargeting.""" + + controller: PinkIKControllerCfg = MISSING + """Configuration for the Pink IK controller that will be used to solve the inverse kinematics.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_task_space_actions.py new file mode 100644 index 00000000000..11c3ff6cedf --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -0,0 +1,211 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import copy +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.controllers.pink_ik import PinkIKController +from isaaclab.managers.action_manager import ActionTerm + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import pink_actions_cfg + + +class PinkInverseKinematicsAction(ActionTerm): + r"""Pink Inverse Kinematics action term. + + This action term processes the action tensor and sets these setpoints in the pink IK framework + The action tensor is ordered in the order of the tasks defined in PinkIKControllerCfg + """ + + cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg + """Configuration for the Pink Inverse Kinematics action term.""" + + _asset: Articulation + """The articulation asset to which the action term is applied.""" + + def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: ManagerBasedEnv): + """Initialize the Pink Inverse Kinematics action term. + + Args: + cfg: The configuration for this action term. + env: The environment in which the action term will be applied. + """ + super().__init__(cfg, env) + + # Resolve joint IDs and names based on the configuration + self._pink_controlled_joint_ids, self._pink_controlled_joint_names = self._asset.find_joints( + self.cfg.pink_controlled_joint_names + ) + self.cfg.controller.joint_names = self._pink_controlled_joint_names + self._hand_joint_ids, self._hand_joint_names = self._asset.find_joints(self.cfg.hand_joint_names) + self._joint_ids = self._pink_controlled_joint_ids + self._hand_joint_ids + self._joint_names = self._pink_controlled_joint_names + self._hand_joint_names + + # Initialize the Pink IK controller + assert env.num_envs > 0, "Number of environments specified are less than 1." + self._ik_controllers = [] + for _ in range(env.num_envs): + self._ik_controllers.append(PinkIKController(cfg=copy.deepcopy(self.cfg.controller), device=self.device)) + + # Create tensors to store raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # Get the simulation time step + self._sim_dt = env.sim.get_physics_dt() + + self.total_time = 0 # Variable to accumulate the total time + self.num_runs = 0 # Counter for the number of runs + + # Save the base_link_frame pose in the world frame as a transformation matrix in + # order to transform the desired pose of the controlled_frame to be with respect to the base_link_frame + # Shape of env.scene[self.cfg.articulation_name].data.body_link_state_w is (num_instances, num_bodies, 13) + base_link_frame_in_world_origin = env.scene[self.cfg.controller.articulation_name].data.body_link_state_w[ + :, + env.scene[self.cfg.controller.articulation_name].data.body_names.index(self.cfg.controller.base_link_name), + :7, + ] + + # Get robot base link frame in env origin frame + base_link_frame_in_env_origin = copy.deepcopy(base_link_frame_in_world_origin) + base_link_frame_in_env_origin[:, :3] -= self._env.scene.env_origins + + self.base_link_frame_in_env_origin = math_utils.make_pose( + base_link_frame_in_env_origin[:, :3], math_utils.matrix_from_quat(base_link_frame_in_env_origin[:, 3:7]) + ) + + # """ + # Properties. + # """ + + @property + def hand_joint_dim(self) -> int: + """Dimension for hand joint positions.""" + return self.cfg.controller.num_hand_joints + + @property + def position_dim(self) -> int: + """Dimension for position (x, y, z).""" + return 3 + + @property + def orientation_dim(self) -> int: + """Dimension for orientation (w, x, y, z).""" + return 4 + + @property + def pose_dim(self) -> int: + """Total pose dimension (position + orientation).""" + return self.position_dim + self.orientation_dim + + @property + def action_dim(self) -> int: + """Dimension of the action space (based on number of tasks and pose dimension).""" + # The tasks for all the controllers are the same, hence just using the first one to calculate the action_dim + return len(self._ik_controllers[0].cfg.variable_input_tasks) * self.pose_dim + self.hand_joint_dim + + @property + def raw_actions(self) -> torch.Tensor: + """Get the raw actions tensor.""" + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + """Get the processed actions tensor.""" + return self._processed_actions + + # """ + # Operations. + # """ + + def process_actions(self, actions: torch.Tensor): + """Process the input actions and set targets for each task. + + Args: + actions: The input actions tensor. + """ + # Store the raw actions + self._raw_actions[:] = actions + self._processed_actions[:] = self.raw_actions + + # Make a copy of actions before modifying so that raw actions are not modified + actions_clone = actions.clone() + + # Extract hand joint positions (last 22 values) + self._target_hand_joint_positions = actions_clone[:, -self.hand_joint_dim :] + + # The action tensor provides the desired pose of the controlled_frame with respect to the env origin frame + # But the pink IK controller expects the desired pose of the controlled_frame with respect to the base_link_frame + # So we need to transform the desired pose of the controlled_frame to be with respect to the base_link_frame + + # Get the controlled_frame pose wrt to the env origin frame + all_controlled_frames_in_env_origin = [] + # The contrllers for all envs are the same, hence just using the first one to get the number of variable_input_tasks + for task_index in range(len(self._ik_controllers[0].cfg.variable_input_tasks)): + controlled_frame_in_env_origin_pos = actions_clone[ + :, task_index * self.pose_dim : task_index * self.pose_dim + self.position_dim + ] + controlled_frame_in_env_origin_quat = actions_clone[ + :, task_index * self.pose_dim + self.position_dim : (task_index + 1) * self.pose_dim + ] + controlled_frame_in_env_origin = math_utils.make_pose( + controlled_frame_in_env_origin_pos, math_utils.matrix_from_quat(controlled_frame_in_env_origin_quat) + ) + all_controlled_frames_in_env_origin.append(controlled_frame_in_env_origin) + # Stack all the controlled_frame poses in the env origin frame. Shape is (num_tasks, num_envs , 4, 4) + all_controlled_frames_in_env_origin = torch.stack(all_controlled_frames_in_env_origin) + + # Transform the controlled_frame to be with respect to the base_link_frame using batched matrix multiplication + controlled_frame_in_base_link_frame = math_utils.pose_in_A_to_pose_in_B( + all_controlled_frames_in_env_origin, math_utils.pose_inv(self.base_link_frame_in_env_origin) + ) + + controlled_frame_in_base_link_frame_pos, controlled_frame_in_base_link_frame_mat = math_utils.unmake_pose( + controlled_frame_in_base_link_frame + ) + + # Loop through each task and set the target + for env_index, ik_controller in enumerate(self._ik_controllers): + for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): + target = task.transform_target_to_world + target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy() + target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy() + task.set_target(target) + + def apply_actions(self): + # start_time = time.time() # Capture the time before the step + """Apply the computed joint positions based on the inverse kinematics solution.""" + all_envs_joint_pos_des = [] + for env_index, ik_controller in enumerate(self._ik_controllers): + curr_joint_pos = self._asset.data.joint_pos[:, self._pink_controlled_joint_ids].cpu().numpy()[env_index] + joint_pos_des = ik_controller.compute(curr_joint_pos, self._sim_dt) + all_envs_joint_pos_des.append(joint_pos_des) + all_envs_joint_pos_des = torch.stack(all_envs_joint_pos_des) + # Combine IK joint positions with hand joint positions + all_envs_joint_pos_des = torch.cat((all_envs_joint_pos_des, self._target_hand_joint_positions), dim=1) + + self._asset.set_joint_position_target(all_envs_joint_pos_des, self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset the action term for specified environments. + + Args: + env_ids: A list of environment IDs to reset. If None, all environments are reset. + """ + self._raw_actions[env_ids] = torch.zeros(self.action_dim, device=self.device) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/task_space_actions.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/task_space_actions.py new file mode 100644 index 00000000000..12067cf65c6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/actions/task_space_actions.py @@ -0,0 +1,705 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log +from pxr import UsdPhysics + +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils +from isaaclab.assets.articulation import Articulation +from isaaclab.controllers.differential_ik import DifferentialIKController +from isaaclab.controllers.operational_space import OperationalSpaceController +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.sensors import ContactSensor, ContactSensorCfg, FrameTransformer, FrameTransformerCfg +from isaaclab.sim.utils import find_matching_prims + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import actions_cfg + + +class DifferentialInverseKinematicsAction(ActionTerm): + r"""Inverse Kinematics action term. + + This action term performs pre-processing of the raw actions using scaling transformation. + + .. math:: + \text{action} = \text{scaling} \times \text{input action} + \text{joint position} = J^{-} \times \text{action} + + where :math:`\text{scaling}` is the scaling applied to the input action, and :math:`\text{input action}` + is the input action from the user, :math:`J` is the Jacobian over the articulation's actuated joints, + and \text{joint position} is the desired joint position command for the articulation's joints. + """ + + cfg: actions_cfg.DifferentialInverseKinematicsActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor + """The scaling factor applied to the input action. Shape is (1, action_dim).""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_joints = len(self._joint_ids) + # parse the body index + body_ids, body_names = self._asset.find_bodies(self.cfg.body_name) + if len(body_ids) != 1: + raise ValueError( + f"Expected one match for the body name: {self.cfg.body_name}. Found {len(body_ids)}: {body_names}." + ) + # save only the first body index + self._body_idx = body_ids[0] + self._body_name = body_names[0] + # check if articulation is fixed-base + # if fixed-base then the jacobian for the base is not computed + # this means that number of bodies is one less than the articulation's number of bodies + if self._asset.is_fixed_base: + self._jacobi_body_idx = self._body_idx - 1 + self._jacobi_joint_ids = self._joint_ids + else: + self._jacobi_body_idx = self._body_idx + self._jacobi_joint_ids = [i + 6 for i in self._joint_ids] + + # log info for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + omni.log.info( + f"Resolved body name for the action term {self.__class__.__name__}: {self._body_name} [{self._body_idx}]" + ) + # Avoid indexing across all joints for efficiency + if self._num_joints == self._asset.num_joints: + self._joint_ids = slice(None) + + # create the differential IK controller + self._ik_controller = DifferentialIKController( + cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device + ) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # save the scale as tensors + self._scale = torch.zeros((self.num_envs, self.action_dim), device=self.device) + self._scale[:] = torch.tensor(self.cfg.scale, device=self.device) + + # convert the fixed offsets to torch tensors of batched shape + if self.cfg.body_offset is not None: + self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1) + self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1) + else: + self._offset_pos, self._offset_rot = None, None + + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return self._ik_controller.action_dim + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + self._processed_actions[:] = self.raw_actions * self._scale + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + # obtain quantities from simulation + ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + # set command into controller + self._ik_controller.set_command(self._processed_actions, ee_pos_curr, ee_quat_curr) + + def apply_actions(self): + # obtain quantities from simulation + ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + joint_pos = self._asset.data.joint_pos[:, self._joint_ids] + # compute the delta in joint-space + if ee_quat_curr.norm() != 0: + jacobian = self._compute_frame_jacobian() + joint_pos_des = self._ik_controller.compute(ee_pos_curr, ee_quat_curr, jacobian, joint_pos) + else: + joint_pos_des = joint_pos.clone() + # set the joint position command + self._asset.set_joint_position_target(joint_pos_des, self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + + """ + Helper functions. + """ + + def _compute_frame_pose(self) -> tuple[torch.Tensor, torch.Tensor]: + """Computes the pose of the target frame in the root frame. + + Returns: + A tuple of the body's position and orientation in the root frame. + """ + # obtain quantities from simulation + ee_pos_w = self._asset.data.body_pos_w[:, self._body_idx] + ee_quat_w = self._asset.data.body_quat_w[:, self._body_idx] + root_pos_w = self._asset.data.root_pos_w + root_quat_w = self._asset.data.root_quat_w + # compute the pose of the body in the root frame + ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) + # account for the offset + if self.cfg.body_offset is not None: + ee_pose_b, ee_quat_b = math_utils.combine_frame_transforms( + ee_pose_b, ee_quat_b, self._offset_pos, self._offset_rot + ) + + return ee_pose_b, ee_quat_b + + def _compute_frame_jacobian(self): + """Computes the geometric Jacobian of the target frame in the root frame. + + This function accounts for the target frame offset and applies the necessary transformations to obtain + the right Jacobian from the parent body Jacobian. + """ + # read the parent jacobian + jacobian = self.jacobian_b + # account for the offset + if self.cfg.body_offset is not None: + # Modify the jacobian to account for the offset + # -- translational part + # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee + # = (v_J_ee + w_J_ee x r_link_ee ) * q + # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q + jacobian[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), jacobian[:, 3:, :]) + # -- rotational part + # w_link = R_link_ee @ w_ee + jacobian[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), jacobian[:, 3:, :]) + + return jacobian + + +class OperationalSpaceControllerAction(ActionTerm): + r"""Operational space controller action term. + + This action term performs pre-processing of the raw actions for operational space control. + + """ + + cfg: actions_cfg.OperationalSpaceControllerActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _contact_sensor: ContactSensor = None + """The contact sensor for the end-effector body.""" + _task_frame_transformer: FrameTransformer = None + """The frame transformer for the task frame.""" + + def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + self._sim_dt = env.sim.get_physics_dt() + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_DoF = len(self._joint_ids) + # parse the ee body index + body_ids, body_names = self._asset.find_bodies(self.cfg.body_name) + if len(body_ids) != 1: + raise ValueError( + f"Expected one match for the ee body name: {self.cfg.body_name}. Found {len(body_ids)}: {body_names}." + ) + # save only the first ee body index + self._ee_body_idx = body_ids[0] + self._ee_body_name = body_names[0] + # check if articulation is fixed-base + # if fixed-base then the jacobian for the base is not computed + # this means that number of bodies is one less than the articulation's number of bodies + if self._asset.is_fixed_base: + self._jacobi_ee_body_idx = self._ee_body_idx - 1 + self._jacobi_joint_idx = self._joint_ids + else: + self._jacobi_ee_body_idx = self._ee_body_idx + self._jacobi_joint_idx = [i + 6 for i in self._joint_ids] + + # log info for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + omni.log.info( + f"Resolved ee body name for the action term {self.__class__.__name__}:" + f" {self._ee_body_name} [{self._ee_body_idx}]" + ) + # Avoid indexing across all joints for efficiency + if self._num_DoF == self._asset.num_joints: + self._joint_ids = slice(None) + + # convert the fixed offsets to torch tensors of batched shape + if self.cfg.body_offset is not None: + self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1) + self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1) + else: + self._offset_pos, self._offset_rot = None, None + + # create contact sensor if any of the command is wrench_abs, and if stiffness is provided + if ( + "wrench_abs" in self.cfg.controller_cfg.target_types + and self.cfg.controller_cfg.contact_wrench_stiffness_task is not None + ): + self._contact_sensor_cfg = ContactSensorCfg(prim_path=self._asset.cfg.prim_path + "/" + self._ee_body_name) + self._contact_sensor = ContactSensor(self._contact_sensor_cfg) + if not self._contact_sensor.is_initialized: + self._contact_sensor._initialize_impl() + self._contact_sensor._is_initialized = True + + # Initialize the task frame transformer if a relative path for the RigidObject, representing the task frame, + # is provided. + if self.cfg.task_frame_rel_path is not None: + # The source RigidObject can be any child of the articulation asset (we will not use it), + # hence, we will use the first RigidObject child. + root_rigidbody_path = self._first_RigidObject_child_path() + task_frame_transformer_path = "/World/envs/env_.*/" + self.cfg.task_frame_rel_path + task_frame_transformer_cfg = FrameTransformerCfg( + prim_path=root_rigidbody_path, + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="task_frame", + prim_path=task_frame_transformer_path, + ), + ], + ) + self._task_frame_transformer = FrameTransformer(task_frame_transformer_cfg) + if not self._task_frame_transformer.is_initialized: + self._task_frame_transformer._initialize_impl() + self._task_frame_transformer._is_initialized = True + # create tensor for task frame pose in the root frame + self._task_frame_pose_b = torch.zeros(self.num_envs, 7, device=self.device) + else: + # create an empty reference for task frame pose + self._task_frame_pose_b = None + + # create the operational space controller + self._osc = OperationalSpaceController(cfg=self.cfg.controller_cfg, num_envs=self.num_envs, device=self.device) + + # create tensors for raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # create tensors for the dynamic-related quantities + self._jacobian_b = torch.zeros(self.num_envs, 6, self._num_DoF, device=self.device) + self._mass_matrix = torch.zeros(self.num_envs, self._num_DoF, self._num_DoF, device=self.device) + self._gravity = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # create tensors for the ee states + self._ee_pose_w = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b_no_offset = torch.zeros(self.num_envs, 7, device=self.device) # The original ee without offset + self._ee_vel_w = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_vel_b = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_force_w = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + self._ee_force_b = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + + # create tensors for the joint states + self._joint_pos = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + self._joint_vel = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # create the joint effort tensor + self._joint_efforts = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # save the scale as tensors + self._position_scale = torch.tensor(self.cfg.position_scale, device=self.device) + self._orientation_scale = torch.tensor(self.cfg.orientation_scale, device=self.device) + self._wrench_scale = torch.tensor(self.cfg.wrench_scale, device=self.device) + self._stiffness_scale = torch.tensor(self.cfg.stiffness_scale, device=self.device) + self._damping_ratio_scale = torch.tensor(self.cfg.damping_ratio_scale, device=self.device) + + # indexes for the various command elements (e.g., pose_rel, stifness, etc.) within the command tensor + self._pose_abs_idx = None + self._pose_rel_idx = None + self._wrench_abs_idx = None + self._stiffness_idx = None + self._damping_ratio_idx = None + self._resolve_command_indexes() + + # Nullspace position control joint targets + self._nullspace_joint_pos_target = None + self._resolve_nullspace_joint_pos_targets() + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the action space of operational space control.""" + return self._osc.action_dim + + @property + def raw_actions(self) -> torch.Tensor: + """Raw actions for operational space control.""" + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + """Processed actions for operational space control.""" + return self._processed_actions + + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_ee_body_idx, :, self._jacobi_joint_idx] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions and sets them as commands for for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + + # Update ee pose, which would be used by relative targets (i.e., pose_rel) + self._compute_ee_pose() + + # Update task frame pose w.r.t. the root frame. + self._compute_task_frame_pose() + + # Pre-process the raw actions for operational space control. + self._preprocess_actions(actions) + + # set command into controller + self._osc.set_command( + command=self._processed_actions, + current_ee_pose_b=self._ee_pose_b, + current_task_frame_pose_b=self._task_frame_pose_b, + ) + + def apply_actions(self): + """Computes the joint efforts for operational space control and applies them to the articulation.""" + + # Update the relevant states and dynamical quantities + self._compute_dynamic_quantities() + self._compute_ee_jacobian() + self._compute_ee_pose() + self._compute_ee_velocity() + self._compute_ee_force() + self._compute_joint_states() + # Calculate the joint efforts + self._joint_efforts[:] = self._osc.compute( + jacobian_b=self._jacobian_b, + current_ee_pose_b=self._ee_pose_b, + current_ee_vel_b=self._ee_vel_b, + current_ee_force_b=self._ee_force_b, + mass_matrix=self._mass_matrix, + gravity=self._gravity, + current_joint_pos=self._joint_pos, + current_joint_vel=self._joint_vel, + nullspace_joint_pos_target=self._nullspace_joint_pos_target, + ) + self._asset.set_joint_effort_target(self._joint_efforts, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Resets the raw actions and the sensors if available. + + Args: + env_ids (Sequence[int] | None): The environment indices to reset. If ``None``, all environments are reset. + """ + self._raw_actions[env_ids] = 0.0 + if self._contact_sensor is not None: + self._contact_sensor.reset(env_ids) + if self._task_frame_transformer is not None: + self._task_frame_transformer.reset(env_ids) + + """ + Helper functions. + + """ + + def _first_RigidObject_child_path(self): + """Finds the first ``RigidObject`` child under the articulation asset. + + Raises: + ValueError: If no child ``RigidObject`` is found under the articulation asset. + + Returns: + str: The path to the first ``RigidObject`` child under the articulation asset. + """ + child_prims = find_matching_prims(self._asset.cfg.prim_path + "/.*") + rigid_child_prim = None + # Loop through the list and stop at the first RigidObject found + for prim in child_prims: + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + rigid_child_prim = prim + break + if rigid_child_prim is None: + raise ValueError("No child rigid body found under the expression: '{self._asset.cfg.prim_path}'/.") + rigid_child_prim_path = rigid_child_prim.GetPath().pathString + # Remove the specific env index from the path string + rigid_child_prim_path = self._asset.cfg.prim_path + "/" + rigid_child_prim_path.split("/")[-1] + return rigid_child_prim_path + + def _resolve_command_indexes(self): + """Resolves the indexes for the various command elements within the command tensor. + + Raises: + ValueError: If any command index is left unresolved. + """ + # First iterate over the target types to find the indexes of the different command elements + cmd_idx = 0 + for target_type in self.cfg.controller_cfg.target_types: + if target_type == "pose_abs": + self._pose_abs_idx = cmd_idx + cmd_idx += 7 + elif target_type == "pose_rel": + self._pose_rel_idx = cmd_idx + cmd_idx += 6 + elif target_type == "wrench_abs": + self._wrench_abs_idx = cmd_idx + cmd_idx += 6 + else: + raise ValueError("Undefined target_type for OSC within OperationalSpaceControllerAction.") + # Then iterate over the impedance parameters depending on the impedance mode + if ( + self.cfg.controller_cfg.impedance_mode == "variable_kp" + or self.cfg.controller_cfg.impedance_mode == "variable" + ): + self._stiffness_idx = cmd_idx + cmd_idx += 6 + if self.cfg.controller_cfg.impedance_mode == "variable": + self._damping_ratio_idx = cmd_idx + cmd_idx += 6 + + # Check if any command is left unresolved + if self.action_dim != cmd_idx: + raise ValueError("Not all command indexes have been resolved.") + + def _resolve_nullspace_joint_pos_targets(self): + """Resolves the nullspace joint pos targets for the operational space controller. + + Raises: + ValueError: If the nullspace joint pos targets are set when null space control is not set to 'position'. + ValueError: If the nullspace joint pos targets are not set when null space control is set to 'position'. + ValueError: If an invalid value is set for nullspace joint pos targets. + """ + + if self.cfg.nullspace_joint_pos_target != "none" and self.cfg.controller_cfg.nullspace_control != "position": + raise ValueError("Nullspace joint targets can only be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "none" and self.cfg.controller_cfg.nullspace_control == "position": + raise ValueError("Nullspace joint targets must be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "zero" or self.cfg.nullspace_joint_pos_target == "none": + # Keep the nullspace joint targets as None as this is later processed as zero in the controller + self._nullspace_joint_pos_target = None + elif self.cfg.nullspace_joint_pos_target == "center": + # Get the center of the robot soft joint limits + self._nullspace_joint_pos_target = torch.mean( + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, :], dim=-1 + ) + elif self.cfg.nullspace_joint_pos_target == "default": + # Get the default joint positions + self._nullspace_joint_pos_target = self._asset.data.default_joint_pos[:, self._joint_ids] + else: + raise ValueError("Invalid value for nullspace joint pos targets.") + + def _compute_dynamic_quantities(self): + """Computes the dynamic quantities for operational space control.""" + + self._mass_matrix[:] = self._asset.root_physx_view.get_generalized_mass_matrices()[:, self._joint_ids, :][ + :, :, self._joint_ids + ] + self._gravity[:] = self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._joint_ids] + + def _compute_ee_jacobian(self): + """Computes the geometric Jacobian of the ee body frame in root frame. + + This function accounts for the target frame offset and applies the necessary transformations to obtain + the right Jacobian from the parent body Jacobian. + """ + # Get the Jacobian in root frame + self._jacobian_b[:] = self.jacobian_b + + # account for the offset + if self.cfg.body_offset is not None: + # Modify the jacobian to account for the offset + # -- translational part + # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee + # = (v_J_ee + w_J_ee x r_link_ee ) * q + # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q + self._jacobian_b[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:, :]) # type: ignore + # -- rotational part + # w_link = R_link_ee @ w_ee + self._jacobian_b[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:, :]) # type: ignore + + def _compute_ee_pose(self): + """Computes the pose of the ee frame in root frame.""" + # Obtain quantities from simulation + self._ee_pose_w[:, 0:3] = self._asset.data.body_pos_w[:, self._ee_body_idx] + self._ee_pose_w[:, 3:7] = self._asset.data.body_quat_w[:, self._ee_body_idx] + # Compute the pose of the ee body in the root frame + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + self._ee_pose_w[:, 0:3], + self._ee_pose_w[:, 3:7], + ) + # Account for the offset + if self.cfg.body_offset is not None: + self._ee_pose_b[:, 0:3], self._ee_pose_b[:, 3:7] = math_utils.combine_frame_transforms( + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7], self._offset_pos, self._offset_rot + ) + else: + self._ee_pose_b[:] = self._ee_pose_b_no_offset + + def _compute_ee_velocity(self): + """Computes the velocity of the ee frame in root frame.""" + # Extract end-effector velocity in the world frame + self._ee_vel_w[:] = self._asset.data.body_vel_w[:, self._ee_body_idx, :] + # Compute the relative velocity in the world frame + relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w + + # Convert ee velocities from world to root frame + self._ee_vel_b[:, 0:3] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3]) + self._ee_vel_b[:, 3:6] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6]) + + # Account for the offset + if self.cfg.body_offset is not None: + # Compute offset vector in root frame + r_offset_b = math_utils.quat_apply(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos) + # Adjust the linear velocity to account for the offset + self._ee_vel_b[:, :3] += torch.cross(self._ee_vel_b[:, 3:], r_offset_b, dim=-1) + # Angular velocity is not affected by the offset + + def _compute_ee_force(self): + """Computes the contact forces on the ee frame in root frame.""" + # Obtain contact forces only if the contact sensor is available + if self._contact_sensor is not None: + self._contact_sensor.update(self._sim_dt) + self._ee_force_w[:] = self._contact_sensor.data.net_forces_w[:, 0, :] # type: ignore + # Rotate forces and torques into root frame + self._ee_force_b[:] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, self._ee_force_w) + + def _compute_joint_states(self): + """Computes the joint states for operational space control.""" + # Extract joint positions and velocities + self._joint_pos[:] = self._asset.data.joint_pos[:, self._joint_ids] + self._joint_vel[:] = self._asset.data.joint_vel[:, self._joint_ids] + + def _compute_task_frame_pose(self): + """Computes the pose of the task frame in root frame.""" + # Update task frame pose if task frame rigidbody is provided + if self._task_frame_transformer is not None and self._task_frame_pose_b is not None: + self._task_frame_transformer.update(self._sim_dt) + # Calculate the pose of the task frame in the root frame + self._task_frame_pose_b[:, :3], self._task_frame_pose_b[:, 3:] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + self._task_frame_transformer.data.target_pos_w[:, 0, :], + self._task_frame_transformer.data.target_quat_w[:, 0, :], + ) + + def _preprocess_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + # Store the raw actions. Please note that the actions contain task space targets + # (in the order of the target_types), and possibly the impedance parameters depending on impedance_mode. + self._raw_actions[:] = actions + # Initialize the processed actions with raw actions. + self._processed_actions[:] = self._raw_actions + # Go through the command types one by one, and apply the pre-processing if needed. + if self._pose_abs_idx is not None: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale + if self._pose_rel_idx is not None: + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale + if self._wrench_abs_idx is not None: + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale + if self._stiffness_idx is not None: + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] *= self._stiffness_scale + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] = torch.clamp( + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6], + min=self.cfg.controller_cfg.motion_stiffness_limits_task[0], + max=self.cfg.controller_cfg.motion_stiffness_limits_task[1], + ) + if self._damping_ratio_idx is not None: + self._processed_actions[ + :, self._damping_ratio_idx : self._damping_ratio_idx + 6 + ] *= self._damping_ratio_scale + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6] = torch.clamp( + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6], + min=self.cfg.controller_cfg.motion_damping_ratio_limits_task[0], + max=self.cfg.controller_cfg.motion_damping_ratio_limits_task[1], + ) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/__init__.py new file mode 100644 index 00000000000..bfe3f9d4c62 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/__init__.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Various command terms that can be used in the environment.""" + +from .commands_cfg import ( + NormalVelocityCommandCfg, + NullCommandCfg, + TerrainBasedPose2dCommandCfg, + UniformPose2dCommandCfg, + UniformPoseCommandCfg, + UniformVelocityCommandCfg, +) +from .null_command import NullCommand +from .pose_2d_command import TerrainBasedPose2dCommand, UniformPose2dCommand +from .pose_command import UniformPoseCommand +from .velocity_command import NormalVelocityCommand, UniformVelocityCommand diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/commands_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/commands_cfg.py new file mode 100644 index 00000000000..34b138d822e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/commands_cfg.py @@ -0,0 +1,253 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING + +from isaaclab.managers import CommandTermCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import BLUE_ARROW_X_MARKER_CFG, FRAME_MARKER_CFG, GREEN_ARROW_X_MARKER_CFG +from isaaclab.utils import configclass + +from .null_command import NullCommand +from .pose_2d_command import TerrainBasedPose2dCommand, UniformPose2dCommand +from .pose_command import UniformPoseCommand +from .velocity_command import NormalVelocityCommand, UniformVelocityCommand + + +@configclass +class NullCommandCfg(CommandTermCfg): + """Configuration for the null command generator.""" + + class_type: type = NullCommand + + def __post_init__(self): + """Post initialization.""" + # set the resampling time range to infinity to avoid resampling + self.resampling_time_range = (math.inf, math.inf) + + +@configclass +class UniformVelocityCommandCfg(CommandTermCfg): + """Configuration for the uniform velocity command generator.""" + + class_type: type = UniformVelocityCommand + + asset_name: str = MISSING + """Name of the asset in the environment for which the commands are generated.""" + + heading_command: bool = False + """Whether to use heading command or angular velocity command. Defaults to False. + + If True, the angular velocity command is computed from the heading error, where the + target heading is sampled uniformly from provided range. Otherwise, the angular velocity + command is sampled uniformly from provided range. + """ + + heading_control_stiffness: float = 1.0 + """Scale factor to convert the heading error to angular velocity command. Defaults to 1.0.""" + + rel_standing_envs: float = 0.0 + """The sampled probability of environments that should be standing still. Defaults to 0.0.""" + + rel_heading_envs: float = 1.0 + """The sampled probability of environments where the robots follow the heading-based angular velocity command + (the others follow the sampled angular velocity command). Defaults to 1.0. + + This parameter is only used if :attr:`heading_command` is True. + """ + + @configclass + class Ranges: + """Uniform distribution ranges for the velocity commands.""" + + lin_vel_x: tuple[float, float] = MISSING + """Range for the linear-x velocity command (in m/s).""" + + lin_vel_y: tuple[float, float] = MISSING + """Range for the linear-y velocity command (in m/s).""" + + ang_vel_z: tuple[float, float] = MISSING + """Range for the angular-z velocity command (in rad/s).""" + + heading: tuple[float, float] | None = None + """Range for the heading command (in rad). Defaults to None. + + This parameter is only used if :attr:`~UniformVelocityCommandCfg.heading_command` is True. + """ + + ranges: Ranges = MISSING + """Distribution ranges for the velocity commands.""" + + goal_vel_visualizer_cfg: VisualizationMarkersCfg = GREEN_ARROW_X_MARKER_CFG.replace( + prim_path="/Visuals/Command/velocity_goal" + ) + """The configuration for the goal velocity visualization marker. Defaults to GREEN_ARROW_X_MARKER_CFG.""" + + current_vel_visualizer_cfg: VisualizationMarkersCfg = BLUE_ARROW_X_MARKER_CFG.replace( + prim_path="/Visuals/Command/velocity_current" + ) + """The configuration for the current velocity visualization marker. Defaults to BLUE_ARROW_X_MARKER_CFG.""" + + # Set the scale of the visualization markers to (0.5, 0.5, 0.5) + goal_vel_visualizer_cfg.markers["arrow"].scale = (0.5, 0.5, 0.5) + current_vel_visualizer_cfg.markers["arrow"].scale = (0.5, 0.5, 0.5) + + +@configclass +class NormalVelocityCommandCfg(UniformVelocityCommandCfg): + """Configuration for the normal velocity command generator.""" + + class_type: type = NormalVelocityCommand + heading_command: bool = False # --> we don't use heading command for normal velocity command. + + @configclass + class Ranges: + """Normal distribution ranges for the velocity commands.""" + + mean_vel: tuple[float, float, float] = MISSING + """Mean velocity for the normal distribution (in m/s). + + The tuple contains the mean linear-x, linear-y, and angular-z velocity. + """ + + std_vel: tuple[float, float, float] = MISSING + """Standard deviation for the normal distribution (in m/s). + + The tuple contains the standard deviation linear-x, linear-y, and angular-z velocity. + """ + + zero_prob: tuple[float, float, float] = MISSING + """Probability of zero velocity for the normal distribution. + + The tuple contains the probability of zero linear-x, linear-y, and angular-z velocity. + """ + + ranges: Ranges = MISSING + """Distribution ranges for the velocity commands.""" + + +@configclass +class UniformPoseCommandCfg(CommandTermCfg): + """Configuration for uniform pose command generator.""" + + class_type: type = UniformPoseCommand + + asset_name: str = MISSING + """Name of the asset in the environment for which the commands are generated.""" + + body_name: str = MISSING + """Name of the body in the asset for which the commands are generated.""" + + make_quat_unique: bool = False + """Whether to make the quaternion unique or not. Defaults to False. + + If True, the quaternion is made unique by ensuring the real part is positive. + """ + + @configclass + class Ranges: + """Uniform distribution ranges for the pose commands.""" + + pos_x: tuple[float, float] = MISSING + """Range for the x position (in m).""" + + pos_y: tuple[float, float] = MISSING + """Range for the y position (in m).""" + + pos_z: tuple[float, float] = MISSING + """Range for the z position (in m).""" + + roll: tuple[float, float] = MISSING + """Range for the roll angle (in rad).""" + + pitch: tuple[float, float] = MISSING + """Range for the pitch angle (in rad).""" + + yaw: tuple[float, float] = MISSING + """Range for the yaw angle (in rad).""" + + ranges: Ranges = MISSING + """Ranges for the commands.""" + + goal_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/goal_pose") + """The configuration for the goal pose visualization marker. Defaults to FRAME_MARKER_CFG.""" + + current_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( + prim_path="/Visuals/Command/body_pose" + ) + """The configuration for the current pose visualization marker. Defaults to FRAME_MARKER_CFG.""" + + # Set the scale of the visualization markers to (0.1, 0.1, 0.1) + goal_pose_visualizer_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + current_pose_visualizer_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + + +@configclass +class UniformPose2dCommandCfg(CommandTermCfg): + """Configuration for the uniform 2D-pose command generator.""" + + class_type: type = UniformPose2dCommand + + asset_name: str = MISSING + """Name of the asset in the environment for which the commands are generated.""" + + simple_heading: bool = MISSING + """Whether to use simple heading or not. + + If True, the heading is in the direction of the target position. + """ + + @configclass + class Ranges: + """Uniform distribution ranges for the position commands.""" + + pos_x: tuple[float, float] = MISSING + """Range for the x position (in m).""" + + pos_y: tuple[float, float] = MISSING + """Range for the y position (in m).""" + + heading: tuple[float, float] = MISSING + """Heading range for the position commands (in rad). + + Used only if :attr:`simple_heading` is False. + """ + + ranges: Ranges = MISSING + """Distribution ranges for the position commands.""" + + goal_pose_visualizer_cfg: VisualizationMarkersCfg = GREEN_ARROW_X_MARKER_CFG.replace( + prim_path="/Visuals/Command/pose_goal" + ) + """The configuration for the goal pose visualization marker. Defaults to GREEN_ARROW_X_MARKER_CFG.""" + + # Set the scale of the visualization markers to (0.2, 0.2, 0.8) + goal_pose_visualizer_cfg.markers["arrow"].scale = (0.2, 0.2, 0.8) + + +@configclass +class TerrainBasedPose2dCommandCfg(UniformPose2dCommandCfg): + """Configuration for the terrain-based position command generator.""" + + class_type = TerrainBasedPose2dCommand + + @configclass + class Ranges: + """Uniform distribution ranges for the position commands.""" + + heading: tuple[float, float] = MISSING + """Heading range for the position commands (in rad). + + Used only if :attr:`simple_heading` is False. + """ + + ranges: Ranges = MISSING + """Distribution ranges for the sampled commands.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/null_command.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/null_command.py new file mode 100644 index 00000000000..66c7f9520fb --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/null_command.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generator that does nothing.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.managers import CommandTerm + +if TYPE_CHECKING: + from .commands_cfg import NullCommandCfg + + +class NullCommand(CommandTerm): + """Command generator that does nothing. + + This command generator does not generate any commands. It is used for environments that do not + require any commands. + """ + + cfg: NullCommandCfg + """Configuration for the command generator.""" + + def __str__(self) -> str: + msg = "NullCommand:\n" + msg += "\tCommand dimension: N/A\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}" + return msg + + """ + Properties + """ + + @property + def command(self): + """Null command. + + Raises: + RuntimeError: No command is generated. Always raises this error. + """ + raise RuntimeError("NullCommandTerm does not generate any commands.") + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + return {} + + def compute(self, dt: float): + pass + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + pass + + def _resample_command(self, env_ids: Sequence[int]): + pass + + def _update_command(self): + pass diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_2d_command.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_2d_command.py new file mode 100644 index 00000000000..3bc46fe5ac7 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_2d_command.py @@ -0,0 +1,208 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for the 2D-pose for locomotion tasks.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation +from isaaclab.managers import CommandTerm +from isaaclab.markers import VisualizationMarkers +from isaaclab.terrains import TerrainImporter +from isaaclab.utils.math import quat_apply_inverse, quat_from_euler_xyz, wrap_to_pi, yaw_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .commands_cfg import TerrainBasedPose2dCommandCfg, UniformPose2dCommandCfg + + +class UniformPose2dCommand(CommandTerm): + """Command generator that generates pose commands containing a 3-D position and heading. + + The command generator samples uniform 2D positions around the environment origin. It sets + the height of the position command to the default root height of the robot. The heading + command is either set to point towards the target or is sampled uniformly. + This can be configured through the :attr:`Pose2dCommandCfg.simple_heading` parameter in + the configuration. + """ + + cfg: UniformPose2dCommandCfg + """Configuration for the command generator.""" + + def __init__(self, cfg: UniformPose2dCommandCfg, env: ManagerBasedEnv): + """Initialize the command generator class. + + Args: + cfg: The configuration parameters for the command generator. + env: The environment object. + """ + # initialize the base class + super().__init__(cfg, env) + + # obtain the robot and terrain assets + # -- robot + self.robot: Articulation = env.scene[cfg.asset_name] + + # crete buffers to store the command + # -- commands: (x, y, z, heading) + self.pos_command_w = torch.zeros(self.num_envs, 3, device=self.device) + self.heading_command_w = torch.zeros(self.num_envs, device=self.device) + self.pos_command_b = torch.zeros_like(self.pos_command_w) + self.heading_command_b = torch.zeros_like(self.heading_command_w) + # -- metrics + self.metrics["error_pos"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["error_heading"] = torch.zeros(self.num_envs, device=self.device) + + def __str__(self) -> str: + msg = "PositionCommand:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}" + return msg + + """ + Properties + """ + + @property + def command(self) -> torch.Tensor: + """The desired 2D-pose in base frame. Shape is (num_envs, 4).""" + return torch.cat([self.pos_command_b, self.heading_command_b.unsqueeze(1)], dim=1) + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + # logs data + self.metrics["error_pos_2d"] = torch.norm(self.pos_command_w[:, :2] - self.robot.data.root_pos_w[:, :2], dim=1) + self.metrics["error_heading"] = torch.abs(wrap_to_pi(self.heading_command_w - self.robot.data.heading_w)) + + def _resample_command(self, env_ids: Sequence[int]): + # obtain env origins for the environments + self.pos_command_w[env_ids] = self._env.scene.env_origins[env_ids] + # offset the position command by the current root position + r = torch.empty(len(env_ids), device=self.device) + self.pos_command_w[env_ids, 0] += r.uniform_(*self.cfg.ranges.pos_x) + self.pos_command_w[env_ids, 1] += r.uniform_(*self.cfg.ranges.pos_y) + self.pos_command_w[env_ids, 2] += self.robot.data.default_root_state[env_ids, 2] + + if self.cfg.simple_heading: + # set heading command to point towards target + target_vec = self.pos_command_w[env_ids] - self.robot.data.root_pos_w[env_ids] + target_direction = torch.atan2(target_vec[:, 1], target_vec[:, 0]) + flipped_target_direction = wrap_to_pi(target_direction + torch.pi) + + # compute errors to find the closest direction to the current heading + # this is done to avoid the discontinuity at the -pi/pi boundary + curr_to_target = wrap_to_pi(target_direction - self.robot.data.heading_w[env_ids]).abs() + curr_to_flipped_target = wrap_to_pi(flipped_target_direction - self.robot.data.heading_w[env_ids]).abs() + + # set the heading command to the closest direction + self.heading_command_w[env_ids] = torch.where( + curr_to_target < curr_to_flipped_target, + target_direction, + flipped_target_direction, + ) + else: + # random heading command + self.heading_command_w[env_ids] = r.uniform_(*self.cfg.ranges.heading) + + def _update_command(self): + """Re-target the position command to the current root state.""" + target_vec = self.pos_command_w - self.robot.data.root_pos_w[:, :3] + self.pos_command_b[:] = quat_apply_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec) + self.heading_command_b[:] = wrap_to_pi(self.heading_command_w - self.robot.data.heading_w) + + def _set_debug_vis_impl(self, debug_vis: bool): + # create markers if necessary for the first tome + if debug_vis: + if not hasattr(self, "goal_pose_visualizer"): + self.goal_pose_visualizer = VisualizationMarkers(self.cfg.goal_pose_visualizer_cfg) + # set their visibility to true + self.goal_pose_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_pose_visualizer"): + self.goal_pose_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # update the box marker + self.goal_pose_visualizer.visualize( + translations=self.pos_command_w, + orientations=quat_from_euler_xyz( + torch.zeros_like(self.heading_command_w), + torch.zeros_like(self.heading_command_w), + self.heading_command_w, + ), + ) + + +class TerrainBasedPose2dCommand(UniformPose2dCommand): + """Command generator that generates pose commands based on the terrain. + + This command generator samples the position commands from the valid patches of the terrain. + The heading commands are either set to point towards the target or are sampled uniformly. + + It expects the terrain to have a valid flat patches under the key 'target'. + """ + + cfg: TerrainBasedPose2dCommandCfg + """Configuration for the command generator.""" + + def __init__(self, cfg: TerrainBasedPose2dCommandCfg, env: ManagerBasedEnv): + # initialize the base class + super().__init__(cfg, env) + + # obtain the terrain asset + self.terrain: TerrainImporter = env.scene["terrain"] + + # obtain the valid targets from the terrain + if "target" not in self.terrain.flat_patches: + raise RuntimeError( + "The terrain-based command generator requires a valid flat patch under 'target' in the terrain." + f" Found: {list(self.terrain.flat_patches.keys())}" + ) + # valid targets: (terrain_level, terrain_type, num_patches, 3) + self.valid_targets: torch.Tensor = self.terrain.flat_patches["target"] + + def _resample_command(self, env_ids: Sequence[int]): + # sample new position targets from the terrain + ids = torch.randint(0, self.valid_targets.shape[2], size=(len(env_ids),), device=self.device) + self.pos_command_w[env_ids] = self.valid_targets[ + self.terrain.terrain_levels[env_ids], self.terrain.terrain_types[env_ids], ids + ] + # offset the position command by the current root height + self.pos_command_w[env_ids, 2] += self.robot.data.default_root_state[env_ids, 2] + + if self.cfg.simple_heading: + # set heading command to point towards target + target_vec = self.pos_command_w[env_ids] - self.robot.data.root_pos_w[env_ids] + target_direction = torch.atan2(target_vec[:, 1], target_vec[:, 0]) + flipped_target_direction = wrap_to_pi(target_direction + torch.pi) + + # compute errors to find the closest direction to the current heading + # this is done to avoid the discontinuity at the -pi/pi boundary + curr_to_target = wrap_to_pi(target_direction - self.robot.data.heading_w[env_ids]).abs() + curr_to_flipped_target = wrap_to_pi(flipped_target_direction - self.robot.data.heading_w[env_ids]).abs() + + # set the heading command to the closest direction + self.heading_command_w[env_ids] = torch.where( + curr_to_target < curr_to_flipped_target, + target_direction, + flipped_target_direction, + ) + else: + # random heading command + r = torch.empty(len(env_ids), device=self.device) + self.heading_command_w[env_ids] = r.uniform_(*self.cfg.ranges.heading) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_command.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_command.py new file mode 100644 index 00000000000..61cbd3fa824 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/pose_command.py @@ -0,0 +1,160 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for pose tracking.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation +from isaaclab.managers import CommandTerm +from isaaclab.markers import VisualizationMarkers +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error, quat_from_euler_xyz, quat_unique + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .commands_cfg import UniformPoseCommandCfg + + +class UniformPoseCommand(CommandTerm): + """Command generator for generating pose commands uniformly. + + The command generator generates poses by sampling positions uniformly within specified + regions in cartesian space. For orientation, it samples uniformly the euler angles + (roll-pitch-yaw) and converts them into quaternion representation (w, x, y, z). + + The position and orientation commands are generated in the base frame of the robot, and not the + simulation world frame. This means that users need to handle the transformation from the + base frame to the simulation world frame themselves. + + .. caution:: + + Sampling orientations uniformly is not strictly the same as sampling euler angles uniformly. + This is because rotations are defined by 3D non-Euclidean space, and the mapping + from euler angles to rotations is not one-to-one. + + """ + + cfg: UniformPoseCommandCfg + """Configuration for the command generator.""" + + def __init__(self, cfg: UniformPoseCommandCfg, env: ManagerBasedEnv): + """Initialize the command generator class. + + Args: + cfg: The configuration parameters for the command generator. + env: The environment object. + """ + # initialize the base class + super().__init__(cfg, env) + + # extract the robot and body index for which the command is generated + self.robot: Articulation = env.scene[cfg.asset_name] + self.body_idx = self.robot.find_bodies(cfg.body_name)[0][0] + + # create buffers + # -- commands: (x, y, z, qw, qx, qy, qz) in root frame + self.pose_command_b = torch.zeros(self.num_envs, 7, device=self.device) + self.pose_command_b[:, 3] = 1.0 + self.pose_command_w = torch.zeros_like(self.pose_command_b) + # -- metrics + self.metrics["position_error"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["orientation_error"] = torch.zeros(self.num_envs, device=self.device) + + def __str__(self) -> str: + msg = "UniformPoseCommand:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}\n" + return msg + + """ + Properties + """ + + @property + def command(self) -> torch.Tensor: + """The desired pose command. Shape is (num_envs, 7). + + The first three elements correspond to the position, followed by the quaternion orientation in (w, x, y, z). + """ + return self.pose_command_b + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + # transform command from base frame to simulation world frame + self.pose_command_w[:, :3], self.pose_command_w[:, 3:] = combine_frame_transforms( + self.robot.data.root_pos_w, + self.robot.data.root_quat_w, + self.pose_command_b[:, :3], + self.pose_command_b[:, 3:], + ) + # compute the error + pos_error, rot_error = compute_pose_error( + self.pose_command_w[:, :3], + self.pose_command_w[:, 3:], + self.robot.data.body_pos_w[:, self.body_idx], + self.robot.data.body_quat_w[:, self.body_idx], + ) + self.metrics["position_error"] = torch.norm(pos_error, dim=-1) + self.metrics["orientation_error"] = torch.norm(rot_error, dim=-1) + + def _resample_command(self, env_ids: Sequence[int]): + # sample new pose targets + # -- position + r = torch.empty(len(env_ids), device=self.device) + self.pose_command_b[env_ids, 0] = r.uniform_(*self.cfg.ranges.pos_x) + self.pose_command_b[env_ids, 1] = r.uniform_(*self.cfg.ranges.pos_y) + self.pose_command_b[env_ids, 2] = r.uniform_(*self.cfg.ranges.pos_z) + # -- orientation + euler_angles = torch.zeros_like(self.pose_command_b[env_ids, :3]) + euler_angles[:, 0].uniform_(*self.cfg.ranges.roll) + euler_angles[:, 1].uniform_(*self.cfg.ranges.pitch) + euler_angles[:, 2].uniform_(*self.cfg.ranges.yaw) + quat = quat_from_euler_xyz(euler_angles[:, 0], euler_angles[:, 1], euler_angles[:, 2]) + # make sure the quaternion has real part as positive + self.pose_command_b[env_ids, 3:] = quat_unique(quat) if self.cfg.make_quat_unique else quat + + def _update_command(self): + pass + + def _set_debug_vis_impl(self, debug_vis: bool): + # create markers if necessary for the first tome + if debug_vis: + if not hasattr(self, "goal_pose_visualizer"): + # -- goal pose + self.goal_pose_visualizer = VisualizationMarkers(self.cfg.goal_pose_visualizer_cfg) + # -- current body pose + self.current_pose_visualizer = VisualizationMarkers(self.cfg.current_pose_visualizer_cfg) + # set their visibility to true + self.goal_pose_visualizer.set_visibility(True) + self.current_pose_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_pose_visualizer"): + self.goal_pose_visualizer.set_visibility(False) + self.current_pose_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # check if robot is initialized + # note: this is needed in-case the robot is de-initialized. we can't access the data + if not self.robot.is_initialized: + return + # update the markers + # -- goal pose + self.goal_pose_visualizer.visualize(self.pose_command_w[:, :3], self.pose_command_w[:, 3:]) + # -- current body pose + body_link_pose_w = self.robot.data.body_link_pose_w[:, self.body_idx] + self.current_pose_visualizer.visualize(body_link_pose_w[:, :3], body_link_pose_w[:, 3:7]) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/velocity_command.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/velocity_command.py new file mode 100644 index 00000000000..7e72c859797 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/commands/velocity_command.py @@ -0,0 +1,292 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for the velocity-based locomotion task.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation +from isaaclab.managers import CommandTerm +from isaaclab.markers import VisualizationMarkers + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .commands_cfg import NormalVelocityCommandCfg, UniformVelocityCommandCfg + + +class UniformVelocityCommand(CommandTerm): + r"""Command generator that generates a velocity command in SE(2) from uniform distribution. + + The command comprises of a linear velocity in x and y direction and an angular velocity around + the z-axis. It is given in the robot's base frame. + + If the :attr:`cfg.heading_command` flag is set to True, the angular velocity is computed from the heading + error similar to doing a proportional control on the heading error. The target heading is sampled uniformly + from the provided range. Otherwise, the angular velocity is sampled uniformly from the provided range. + + Mathematically, the angular velocity is computed as follows from the heading command: + + .. math:: + + \omega_z = \frac{1}{2} \text{wrap_to_pi}(\theta_{\text{target}} - \theta_{\text{current}}) + + """ + + cfg: UniformVelocityCommandCfg + """The configuration of the command generator.""" + + def __init__(self, cfg: UniformVelocityCommandCfg, env: ManagerBasedEnv): + """Initialize the command generator. + + Args: + cfg: The configuration of the command generator. + env: The environment. + + Raises: + ValueError: If the heading command is active but the heading range is not provided. + """ + # initialize the base class + super().__init__(cfg, env) + + # check configuration + if self.cfg.heading_command and self.cfg.ranges.heading is None: + raise ValueError( + "The velocity command has heading commands active (heading_command=True) but the `ranges.heading`" + " parameter is set to None." + ) + if self.cfg.ranges.heading and not self.cfg.heading_command: + omni.log.warn( + f"The velocity command has the 'ranges.heading' attribute set to '{self.cfg.ranges.heading}'" + " but the heading command is not active. Consider setting the flag for the heading command to True." + ) + + # obtain the robot asset + # -- robot + self.robot: Articulation = env.scene[cfg.asset_name] + + # crete buffers to store the command + # -- command: x vel, y vel, yaw vel, heading + self.vel_command_b = torch.zeros(self.num_envs, 3, device=self.device) + self.heading_target = torch.zeros(self.num_envs, device=self.device) + self.is_heading_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self.is_standing_env = torch.zeros_like(self.is_heading_env) + # -- metrics + self.metrics["error_vel_xy"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["error_vel_yaw"] = torch.zeros(self.num_envs, device=self.device) + + def __str__(self) -> str: + """Return a string representation of the command generator.""" + msg = "UniformVelocityCommand:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}\n" + msg += f"\tHeading command: {self.cfg.heading_command}\n" + if self.cfg.heading_command: + msg += f"\tHeading probability: {self.cfg.rel_heading_envs}\n" + msg += f"\tStanding probability: {self.cfg.rel_standing_envs}" + return msg + + """ + Properties + """ + + @property + def command(self) -> torch.Tensor: + """The desired base velocity command in the base frame. Shape is (num_envs, 3).""" + return self.vel_command_b + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + # time for which the command was executed + max_command_time = self.cfg.resampling_time_range[1] + max_command_step = max_command_time / self._env.step_dt + # logs data + self.metrics["error_vel_xy"] += ( + torch.norm(self.vel_command_b[:, :2] - self.robot.data.root_lin_vel_b[:, :2], dim=-1) / max_command_step + ) + self.metrics["error_vel_yaw"] += ( + torch.abs(self.vel_command_b[:, 2] - self.robot.data.root_ang_vel_b[:, 2]) / max_command_step + ) + + def _resample_command(self, env_ids: Sequence[int]): + # sample velocity commands + r = torch.empty(len(env_ids), device=self.device) + # -- linear velocity - x direction + self.vel_command_b[env_ids, 0] = r.uniform_(*self.cfg.ranges.lin_vel_x) + # -- linear velocity - y direction + self.vel_command_b[env_ids, 1] = r.uniform_(*self.cfg.ranges.lin_vel_y) + # -- ang vel yaw - rotation around z + self.vel_command_b[env_ids, 2] = r.uniform_(*self.cfg.ranges.ang_vel_z) + # heading target + if self.cfg.heading_command: + self.heading_target[env_ids] = r.uniform_(*self.cfg.ranges.heading) + # update heading envs + self.is_heading_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_heading_envs + # update standing envs + self.is_standing_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_standing_envs + + def _update_command(self): + """Post-processes the velocity command. + + This function sets velocity command to zero for standing environments and computes angular + velocity from heading direction if the heading_command flag is set. + """ + # Compute angular velocity from heading direction + if self.cfg.heading_command: + # resolve indices of heading envs + env_ids = self.is_heading_env.nonzero(as_tuple=False).flatten() + # compute angular velocity + heading_error = math_utils.wrap_to_pi(self.heading_target[env_ids] - self.robot.data.heading_w[env_ids]) + self.vel_command_b[env_ids, 2] = torch.clip( + self.cfg.heading_control_stiffness * heading_error, + min=self.cfg.ranges.ang_vel_z[0], + max=self.cfg.ranges.ang_vel_z[1], + ) + # Enforce standing (i.e., zero velocity command) for standing envs + # TODO: check if conversion is needed + standing_env_ids = self.is_standing_env.nonzero(as_tuple=False).flatten() + self.vel_command_b[standing_env_ids, :] = 0.0 + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first tome + if not hasattr(self, "goal_vel_visualizer"): + # -- goal + self.goal_vel_visualizer = VisualizationMarkers(self.cfg.goal_vel_visualizer_cfg) + # -- current + self.current_vel_visualizer = VisualizationMarkers(self.cfg.current_vel_visualizer_cfg) + # set their visibility to true + self.goal_vel_visualizer.set_visibility(True) + self.current_vel_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_vel_visualizer"): + self.goal_vel_visualizer.set_visibility(False) + self.current_vel_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # check if robot is initialized + # note: this is needed in-case the robot is de-initialized. we can't access the data + if not self.robot.is_initialized: + return + # get marker location + # -- base state + base_pos_w = self.robot.data.root_pos_w.clone() + base_pos_w[:, 2] += 0.5 + # -- resolve the scales and quaternions + vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.command[:, :2]) + vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_b[:, :2]) + # display markers + self.goal_vel_visualizer.visualize(base_pos_w, vel_des_arrow_quat, vel_des_arrow_scale) + self.current_vel_visualizer.visualize(base_pos_w, vel_arrow_quat, vel_arrow_scale) + + """ + Internal helpers. + """ + + def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Converts the XY base velocity command to arrow direction rotation.""" + # obtain default scale of the marker + default_scale = self.goal_vel_visualizer.cfg.markers["arrow"].scale + # arrow-scale + arrow_scale = torch.tensor(default_scale, device=self.device).repeat(xy_velocity.shape[0], 1) + arrow_scale[:, 0] *= torch.linalg.norm(xy_velocity, dim=1) * 3.0 + # arrow-direction + heading_angle = torch.atan2(xy_velocity[:, 1], xy_velocity[:, 0]) + zeros = torch.zeros_like(heading_angle) + arrow_quat = math_utils.quat_from_euler_xyz(zeros, zeros, heading_angle) + # convert everything back from base to world frame + base_quat_w = self.robot.data.root_quat_w + arrow_quat = math_utils.quat_mul(base_quat_w, arrow_quat) + + return arrow_scale, arrow_quat + + +class NormalVelocityCommand(UniformVelocityCommand): + """Command generator that generates a velocity command in SE(2) from a normal distribution. + + The command comprises of a linear velocity in x and y direction and an angular velocity around + the z-axis. It is given in the robot's base frame. + + The command is sampled from a normal distribution with mean and standard deviation specified in + the configuration. With equal probability, the sign of the individual components is flipped. + """ + + cfg: NormalVelocityCommandCfg + """The command generator configuration.""" + + def __init__(self, cfg: NormalVelocityCommandCfg, env: ManagerBasedEnv): + """Initializes the command generator. + + Args: + cfg: The command generator configuration. + env: The environment. + """ + super().__init__(cfg, env) + # create buffers for zero commands envs + self.is_zero_vel_x_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self.is_zero_vel_y_env = torch.zeros_like(self.is_zero_vel_x_env) + self.is_zero_vel_yaw_env = torch.zeros_like(self.is_zero_vel_x_env) + + def __str__(self) -> str: + """Return a string representation of the command generator.""" + msg = "NormalVelocityCommand:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}\n" + msg += f"\tStanding probability: {self.cfg.rel_standing_envs}" + return msg + + def _resample_command(self, env_ids): + # sample velocity commands + r = torch.empty(len(env_ids), device=self.device) + # -- linear velocity - x direction + self.vel_command_b[env_ids, 0] = r.normal_(mean=self.cfg.ranges.mean_vel[0], std=self.cfg.ranges.std_vel[0]) + self.vel_command_b[env_ids, 0] *= torch.where(r.uniform_(0.0, 1.0) <= 0.5, 1.0, -1.0) + # -- linear velocity - y direction + self.vel_command_b[env_ids, 1] = r.normal_(mean=self.cfg.ranges.mean_vel[1], std=self.cfg.ranges.std_vel[1]) + self.vel_command_b[env_ids, 1] *= torch.where(r.uniform_(0.0, 1.0) <= 0.5, 1.0, -1.0) + # -- angular velocity - yaw direction + self.vel_command_b[env_ids, 2] = r.normal_(mean=self.cfg.ranges.mean_vel[2], std=self.cfg.ranges.std_vel[2]) + self.vel_command_b[env_ids, 2] *= torch.where(r.uniform_(0.0, 1.0) <= 0.5, 1.0, -1.0) + + # update element wise zero velocity command + # TODO what is zero prob ? + self.is_zero_vel_x_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.ranges.zero_prob[0] + self.is_zero_vel_y_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.ranges.zero_prob[1] + self.is_zero_vel_yaw_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.ranges.zero_prob[2] + + # update standing envs + self.is_standing_env[env_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_standing_envs + + def _update_command(self): + """Sets velocity command to zero for standing envs.""" + # Enforce standing (i.e., zero velocity command) for standing envs + standing_env_ids = self.is_standing_env.nonzero(as_tuple=False).flatten() # TODO check if conversion is needed + self.vel_command_b[standing_env_ids, :] = 0.0 + + # Enforce zero velocity for individual elements + # TODO: check if conversion is needed + zero_vel_x_env_ids = self.is_zero_vel_x_env.nonzero(as_tuple=False).flatten() + zero_vel_y_env_ids = self.is_zero_vel_y_env.nonzero(as_tuple=False).flatten() + zero_vel_yaw_env_ids = self.is_zero_vel_yaw_env.nonzero(as_tuple=False).flatten() + self.vel_command_b[zero_vel_x_env_ids, 0] = 0.0 + self.vel_command_b[zero_vel_y_env_ids, 1] = 0.0 + self.vel_command_b[zero_vel_yaw_env_ids, 2] = 0.0 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/curriculums.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/curriculums.py new file mode 100644 index 00000000000..df6c2c9db0d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/curriculums.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create curriculum for the learning environment. + +The functions can be passed to the :class:`isaaclab.managers.CurriculumTermCfg` object to enable +the curriculum introduced by the function. +""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def modify_reward_weight(env: ManagerBasedRLEnv, env_ids: Sequence[int], term_name: str, weight: float, num_steps: int): + """Curriculum that modifies a reward weight a given number of steps. + + Args: + env: The learning environment. + env_ids: Not used since all environments are affected. + term_name: The name of the reward term. + weight: The weight of the reward term. + num_steps: The number of steps after which the change should be applied. + """ + if env.common_step_counter > num_steps: + # obtain term settings + term_cfg = env.reward_manager.get_term_cfg(term_name) + # update term settings + term_cfg.weight = weight + env.reward_manager.set_term_cfg(term_name, term_cfg) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/events.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/events.py new file mode 100644 index 00000000000..9f96f2a85a8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/events.py @@ -0,0 +1,1435 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to enable different events. + +Events include anything related to altering the simulation state. This includes changing the physics +materials, applying external forces, and resetting the state of the asset. + +The functions can be passed to the :class:`isaaclab.managers.EventTermCfg` object to enable +the event introduced by the function. +""" + +from __future__ import annotations + +import math +import torch +from typing import TYPE_CHECKING, Literal + +import carb +import omni.physics.tensors.impl.api as physx +import omni.usd +from isaacsim.core.utils.extensions import enable_extension +from pxr import Gf, Sdf, UsdGeom, Vt + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.actuators import ImplicitActuator +from isaaclab.assets import Articulation, DeformableObject, RigidObject +from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg +from isaaclab.terrains import TerrainImporter + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def randomize_rigid_body_scale( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + scale_range: tuple[float, float] | dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg, + relative_child_path: str | None = None, +): + """Randomize the scale of a rigid body asset in the USD stage. + + This function modifies the "xformOp:scale" property of all the prims corresponding to the asset. + + It takes a tuple or dictionary for the scale ranges. If it is a tuple, then the scaling along + individual axis is performed equally. If it is a dictionary, the scaling is independent across each dimension. + The keys of the dictionary are ``x``, ``y``, and ``z``. The values are tuples of the form ``(min, max)``. + + If the dictionary does not contain a key, the range is set to one for that axis. + + Relative child path can be used to randomize the scale of a specific child prim of the asset. + For example, if the asset at prim path expression "/World/envs/env_.*/Object" has a child + with the path "/World/envs/env_.*/Object/mesh", then the relative child path should be "mesh" or + "/mesh". + + .. attention:: + Since this function modifies USD properties that are parsed by the physics engine once the simulation + starts, the term should only be used before the simulation starts playing. This corresponds to the + event mode named "usd". Using it at simulation time, may lead to unpredictable behaviors. + + .. note:: + When randomizing the scale of individual assets, please make sure to set + :attr:`isaaclab.scene.InteractiveSceneCfg.replicate_physics` to False. This ensures that physics + parser will parse the individual asset properties separately. + """ + # check if sim is running + if env.sim.is_playing(): + raise RuntimeError( + "Randomizing scale while simulation is running leads to unpredictable behaviors." + " Please ensure that the event term is called before the simulation starts by using the 'usd' mode." + ) + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + if isinstance(asset, Articulation): + raise ValueError( + "Scaling an articulation randomly is not supported, as it affects joint attributes and can cause" + " unexpected behavior. To achieve different scales, we recommend generating separate USD files for" + " each version of the articulation and using multi-asset spawning. For more details, refer to:" + " https://isaac-sim.github.io/IsaacLab/main/source/how-to/multi_asset_spawning.html" + ) + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu") + else: + env_ids = env_ids.cpu() + + # acquire stage + stage = omni.usd.get_context().get_stage() + # resolve prim paths for spawning and cloning + prim_paths = sim_utils.find_matching_prim_paths(asset.cfg.prim_path) + + # sample scale values + if isinstance(scale_range, dict): + range_list = [scale_range.get(key, (1.0, 1.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device="cpu") + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device="cpu") + else: + rand_samples = math_utils.sample_uniform(*scale_range, (len(env_ids), 1), device="cpu") + rand_samples = rand_samples.repeat(1, 3) + # convert to list for the for loop + rand_samples = rand_samples.tolist() + + # apply the randomization to the parent if no relative child path is provided + # this might be useful if user wants to randomize a particular mesh in the prim hierarchy + if relative_child_path is None: + relative_child_path = "" + elif not relative_child_path.startswith("/"): + relative_child_path = "/" + relative_child_path + + # use sdf changeblock for faster processing of USD properties + with Sdf.ChangeBlock(): + for i, env_id in enumerate(env_ids): + # path to prim to randomize + prim_path = prim_paths[env_id] + relative_child_path + # spawn single instance + prim_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_path) + + # get the attribute to randomize + scale_spec = prim_spec.GetAttributeAtPath(prim_path + ".xformOp:scale") + # if the scale attribute does not exist, create it + has_scale_attr = scale_spec is not None + if not has_scale_attr: + scale_spec = Sdf.AttributeSpec(prim_spec, prim_path + ".xformOp:scale", Sdf.ValueTypeNames.Double3) + + # set the new scale + scale_spec.default = Gf.Vec3f(*rand_samples[i]) + + # ensure the operation is done in the right ordering if we created the scale attribute. + # otherwise, we assume the scale attribute is already in the right order. + # note: by default isaac sim follows this ordering for the transform stack so any asset + # created through it will have the correct ordering + if not has_scale_attr: + op_order_spec = prim_spec.GetAttributeAtPath(prim_path + ".xformOpOrder") + if op_order_spec is None: + op_order_spec = Sdf.AttributeSpec( + prim_spec, UsdGeom.Tokens.xformOpOrder, Sdf.ValueTypeNames.TokenArray + ) + op_order_spec.default = Vt.TokenArray(["xformOp:translate", "xformOp:orient", "xformOp:scale"]) + + +class randomize_rigid_body_material(ManagerTermBase): + """Randomize the physics materials on all geometries of the asset. + + This function creates a set of physics materials with random static friction, dynamic friction, and restitution + values. The number of materials is specified by ``num_buckets``. The materials are generated by sampling + uniform random values from the given ranges. + + The material properties are then assigned to the geometries of the asset. The assignment is done by + creating a random integer tensor of shape (num_instances, max_num_shapes) where ``num_instances`` + is the number of assets spawned and ``max_num_shapes`` is the maximum number of shapes in the asset (over + all bodies). The integer values are used as indices to select the material properties from the + material buckets. + + If the flag ``make_consistent`` is set to ``True``, the dynamic friction is set to be less than or equal to + the static friction. This obeys the physics constraint on friction values. However, it may not always be + essential for the application. Thus, the flag is set to ``False`` by default. + + .. attention:: + This function uses CPU tensors to assign the material properties. It is recommended to use this function + only during the initialization of the environment. Otherwise, it may lead to a significant performance + overhead. + + .. note:: + PhysX only allows 64000 unique physics materials in the scene. If the number of materials exceeds this + limit, the simulation will crash. Due to this reason, we sample the materials only once during initialization. + Afterwards, these materials are randomly assigned to the geometries of the asset. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + ValueError: If the asset is not a RigidObject or an Articulation. + """ + super().__init__(cfg, env) + + # extract the used quantities (to enable type-hinting) + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + + if not isinstance(self.asset, (RigidObject, Articulation)): + raise ValueError( + f"Randomization term 'randomize_rigid_body_material' not supported for asset: '{self.asset_cfg.name}'" + f" with type: '{type(self.asset)}'." + ) + + # obtain number of shapes per body (needed for indexing the material properties correctly) + # note: this is a workaround since the Articulation does not provide a direct way to obtain the number of shapes + # per body. We use the physics simulation view to obtain the number of shapes per body. + if isinstance(self.asset, Articulation) and self.asset_cfg.body_ids != slice(None): + self.num_shapes_per_body = [] + for link_path in self.asset.root_physx_view.link_paths[0]: + link_physx_view = self.asset._physics_sim_view.create_rigid_body_view(link_path) # type: ignore + self.num_shapes_per_body.append(link_physx_view.max_shapes) + # ensure the parsing is correct + num_shapes = sum(self.num_shapes_per_body) + expected_shapes = self.asset.root_physx_view.max_shapes + if num_shapes != expected_shapes: + raise ValueError( + "Randomization term 'randomize_rigid_body_material' failed to parse the number of shapes per body." + f" Expected total shapes: {expected_shapes}, but got: {num_shapes}." + ) + else: + # in this case, we don't need to do special indexing + self.num_shapes_per_body = None + + # obtain parameters for sampling friction and restitution values + static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) + dynamic_friction_range = cfg.params.get("dynamic_friction_range", (1.0, 1.0)) + restitution_range = cfg.params.get("restitution_range", (0.0, 0.0)) + num_buckets = int(cfg.params.get("num_buckets", 1)) + + # sample material properties from the given ranges + # note: we only sample the materials once during initialization + # afterwards these are randomly assigned to the geometries of the asset + range_list = [static_friction_range, dynamic_friction_range, restitution_range] + ranges = torch.tensor(range_list, device="cpu") + self.material_buckets = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (num_buckets, 3), device="cpu") + + # ensure dynamic friction is always less than static friction + make_consistent = cfg.params.get("make_consistent", False) + if make_consistent: + self.material_buckets[:, 1] = torch.min(self.material_buckets[:, 0], self.material_buckets[:, 1]) + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + static_friction_range: tuple[float, float], + dynamic_friction_range: tuple[float, float], + restitution_range: tuple[float, float], + num_buckets: int, + asset_cfg: SceneEntityCfg, + make_consistent: bool = False, + ): + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu") + else: + env_ids = env_ids.cpu() + + # randomly assign material IDs to the geometries + total_num_shapes = self.asset.root_physx_view.max_shapes + bucket_ids = torch.randint(0, num_buckets, (len(env_ids), total_num_shapes), device="cpu") + material_samples = self.material_buckets[bucket_ids] + + # retrieve material buffer from the physics simulation + materials = self.asset.root_physx_view.get_material_properties() + + # update material buffer with new samples + if self.num_shapes_per_body is not None: + # sample material properties from the given ranges + for body_id in self.asset_cfg.body_ids: + # obtain indices of shapes for the body + start_idx = sum(self.num_shapes_per_body[:body_id]) + end_idx = start_idx + self.num_shapes_per_body[body_id] + # assign the new materials + # material samples are of shape: num_env_ids x total_num_shapes x 3 + materials[env_ids, start_idx:end_idx] = material_samples[:, start_idx:end_idx] + else: + # assign all the materials + materials[env_ids] = material_samples[:] + + # apply to simulation + self.asset.root_physx_view.set_material_properties(materials, env_ids) + + +def randomize_rigid_body_mass( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + mass_distribution_params: tuple[float, float], + operation: Literal["add", "scale", "abs"], + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", + recompute_inertia: bool = True, +): + """Randomize the mass of the bodies by adding, scaling, or setting random values. + + This function allows randomizing the mass of the bodies of the asset. The function samples random values from the + given distribution parameters and adds, scales, or sets the values into the physics simulation based on the operation. + + If the ``recompute_inertia`` flag is set to ``True``, the function recomputes the inertia tensor of the bodies + after setting the mass. This is useful when the mass is changed significantly, as the inertia tensor depends + on the mass. It assumes the body is a uniform density object. If the body is not a uniform density object, + the inertia tensor may not be accurate. + + .. tip:: + This function uses CPU tensors to assign the body masses. It is recommended to use this function + only during the initialization of the environment. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu") + else: + env_ids = env_ids.cpu() + + # resolve body indices + if asset_cfg.body_ids == slice(None): + body_ids = torch.arange(asset.num_bodies, dtype=torch.int, device="cpu") + else: + body_ids = torch.tensor(asset_cfg.body_ids, dtype=torch.int, device="cpu") + + # get the current masses of the bodies (num_assets, num_bodies) + masses = asset.root_physx_view.get_masses() + + # apply randomization on default values + # this is to make sure when calling the function multiple times, the randomization is applied on the + # default values and not the previously randomized values + masses[env_ids[:, None], body_ids] = asset.data.default_mass[env_ids[:, None], body_ids].clone() + + # sample from the given range + # note: we modify the masses in-place for all environments + # however, the setter takes care that only the masses of the specified environments are modified + masses = _randomize_prop_by_op( + masses, mass_distribution_params, env_ids, body_ids, operation=operation, distribution=distribution + ) + + # set the mass into the physics simulation + asset.root_physx_view.set_masses(masses, env_ids) + + # recompute inertia tensors if needed + if recompute_inertia: + # compute the ratios of the new masses to the initial masses + ratios = masses[env_ids[:, None], body_ids] / asset.data.default_mass[env_ids[:, None], body_ids] + # scale the inertia tensors by the the ratios + # since mass randomization is done on default values, we can use the default inertia tensors + inertias = asset.root_physx_view.get_inertias() + if isinstance(asset, Articulation): + # inertia has shape: (num_envs, num_bodies, 9) for articulation + inertias[env_ids[:, None], body_ids] = ( + asset.data.default_inertia[env_ids[:, None], body_ids] * ratios[..., None] + ) + else: + # inertia has shape: (num_envs, 9) for rigid object + inertias[env_ids] = asset.data.default_inertia[env_ids] * ratios + # set the inertia tensors into the physics simulation + asset.root_physx_view.set_inertias(inertias, env_ids) + + +def randomize_rigid_body_com( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + com_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg, +): + """Randomize the center of mass (CoM) of rigid bodies by adding a random value sampled from the given ranges. + + .. note:: + This function uses CPU tensors to assign the CoM. It is recommended to use this function + only during the initialization of the environment. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu") + else: + env_ids = env_ids.cpu() + + # resolve body indices + if asset_cfg.body_ids == slice(None): + body_ids = torch.arange(asset.num_bodies, dtype=torch.int, device="cpu") + else: + body_ids = torch.tensor(asset_cfg.body_ids, dtype=torch.int, device="cpu") + + # sample random CoM values + range_list = [com_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device="cpu") + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device="cpu").unsqueeze(1) + + # get the current com of the bodies (num_assets, num_bodies) + coms = asset.root_physx_view.get_coms().clone() + + # Randomize the com in range + coms[:, body_ids, :3] += rand_samples + + # Set the new coms + asset.root_physx_view.set_coms(coms, env_ids) + + +def randomize_rigid_body_collider_offsets( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + rest_offset_distribution_params: tuple[float, float] | None = None, + contact_offset_distribution_params: tuple[float, float] | None = None, + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", +): + """Randomize the collider parameters of rigid bodies in an asset by adding, scaling, or setting random values. + + This function allows randomizing the collider parameters of the asset, such as rest and contact offsets. + These correspond to the physics engine collider properties that affect the collision checking. + + The function samples random values from the given distribution parameters and applies the operation to + the collider properties. It then sets the values into the physics simulation. If the distribution parameters + are not provided for a particular property, the function does not modify the property. + + Currently, the distribution parameters are applied as absolute values. + + .. tip:: + This function uses CPU tensors to assign the collision properties. It is recommended to use this function + only during the initialization of the environment. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu") + + # sample collider properties from the given ranges and set into the physics simulation + # -- rest offsets + if rest_offset_distribution_params is not None: + rest_offset = asset.root_physx_view.get_rest_offsets().clone() + rest_offset = _randomize_prop_by_op( + rest_offset, + rest_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + asset.root_physx_view.set_rest_offsets(rest_offset, env_ids.cpu()) + # -- contact offsets + if contact_offset_distribution_params is not None: + contact_offset = asset.root_physx_view.get_contact_offsets().clone() + contact_offset = _randomize_prop_by_op( + contact_offset, + contact_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + asset.root_physx_view.set_contact_offsets(contact_offset, env_ids.cpu()) + + +def randomize_physics_scene_gravity( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + gravity_distribution_params: tuple[list[float], list[float]], + operation: Literal["add", "scale", "abs"], + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", +): + """Randomize gravity by adding, scaling, or setting random values. + + This function allows randomizing gravity of the physics scene. The function samples random values from the + given distribution parameters and adds, scales, or sets the values into the physics simulation based on the + operation. + + The distribution parameters are lists of two elements each, representing the lower and upper bounds of the + distribution for the x, y, and z components of the gravity vector. The function samples random values for each + component independently. + + .. attention:: + This function applied the same gravity for all the environments. + + .. tip:: + This function uses CPU tensors to assign gravity. + """ + # get the current gravity + gravity = torch.tensor(env.sim.cfg.gravity, device="cpu").unsqueeze(0) + dist_param_0 = torch.tensor(gravity_distribution_params[0], device="cpu") + dist_param_1 = torch.tensor(gravity_distribution_params[1], device="cpu") + gravity = _randomize_prop_by_op( + gravity, + (dist_param_0, dist_param_1), + None, + slice(None), + operation=operation, + distribution=distribution, + ) + # unbatch the gravity tensor into a list + gravity = gravity[0].tolist() + + # set the gravity into the physics simulation + physics_sim_view: physx.SimulationView = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view.set_gravity(carb.Float3(*gravity)) + + +def randomize_actuator_gains( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + stiffness_distribution_params: tuple[float, float] | None = None, + damping_distribution_params: tuple[float, float] | None = None, + operation: Literal["add", "scale", "abs"] = "abs", + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", +): + """Randomize the actuator gains in an articulation by adding, scaling, or setting random values. + + This function allows randomizing the actuator stiffness and damping gains. + + The function samples random values from the given distribution parameters and applies the operation to the joint properties. + It then sets the values into the actuator models. If the distribution parameters are not provided for a particular property, + the function does not modify the property. + + .. tip:: + For implicit actuators, this function uses CPU tensors to assign the actuator gains into the simulation. + In such cases, it is recommended to use this function only during the initialization of the environment. + """ + # Extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # Resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=asset.device) + + def randomize(data: torch.Tensor, params: tuple[float, float]) -> torch.Tensor: + return _randomize_prop_by_op( + data, params, dim_0_ids=None, dim_1_ids=actuator_indices, operation=operation, distribution=distribution + ) + + # Loop through actuators and randomize gains + for actuator in asset.actuators.values(): + if isinstance(asset_cfg.joint_ids, slice): + # we take all the joints of the actuator + actuator_indices = slice(None) + if isinstance(actuator.joint_indices, slice): + global_indices = slice(None) + else: + global_indices = torch.tensor(actuator.joint_indices, device=asset.device) + elif isinstance(actuator.joint_indices, slice): + # we take the joints defined in the asset config + global_indices = actuator_indices = torch.tensor(asset_cfg.joint_ids, device=asset.device) + else: + # we take the intersection of the actuator joints and the asset config joints + actuator_joint_indices = torch.tensor(actuator.joint_indices, device=asset.device) + asset_joint_ids = torch.tensor(asset_cfg.joint_ids, device=asset.device) + # the indices of the joints in the actuator that have to be randomized + actuator_indices = torch.nonzero(torch.isin(actuator_joint_indices, asset_joint_ids)).view(-1) + if len(actuator_indices) == 0: + continue + # maps actuator indices that have to be randomized to global joint indices + global_indices = actuator_joint_indices[actuator_indices] + # Randomize stiffness + if stiffness_distribution_params is not None: + stiffness = actuator.stiffness[env_ids].clone() + stiffness[:, actuator_indices] = asset.data.default_joint_stiffness[env_ids][:, global_indices].clone() + randomize(stiffness, stiffness_distribution_params) + actuator.stiffness[env_ids] = stiffness + if isinstance(actuator, ImplicitActuator): + asset.write_joint_stiffness_to_sim(stiffness, joint_ids=actuator.joint_indices, env_ids=env_ids) + # Randomize damping + if damping_distribution_params is not None: + damping = actuator.damping[env_ids].clone() + damping[:, actuator_indices] = asset.data.default_joint_damping[env_ids][:, global_indices].clone() + randomize(damping, damping_distribution_params) + actuator.damping[env_ids] = damping + if isinstance(actuator, ImplicitActuator): + asset.write_joint_damping_to_sim(damping, joint_ids=actuator.joint_indices, env_ids=env_ids) + + +def randomize_joint_parameters( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + friction_distribution_params: tuple[float, float] | None = None, + armature_distribution_params: tuple[float, float] | None = None, + lower_limit_distribution_params: tuple[float, float] | None = None, + upper_limit_distribution_params: tuple[float, float] | None = None, + operation: Literal["add", "scale", "abs"] = "abs", + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", +): + """Randomize the simulated joint parameters of an articulation by adding, scaling, or setting random values. + + This function allows randomizing the joint parameters of the asset. These correspond to the physics engine + joint properties that affect the joint behavior. The properties include the joint friction coefficient, armature, + and joint position limits. + + The function samples random values from the given distribution parameters and applies the operation to the + joint properties. It then sets the values into the physics simulation. If the distribution parameters are + not provided for a particular property, the function does not modify the property. + + .. tip:: + This function uses CPU tensors to assign the joint properties. It is recommended to use this function + only during the initialization of the environment. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=asset.device) + + # resolve joint indices + if asset_cfg.joint_ids == slice(None): + joint_ids = slice(None) # for optimization purposes + else: + joint_ids = torch.tensor(asset_cfg.joint_ids, dtype=torch.int, device=asset.device) + + # sample joint properties from the given ranges and set into the physics simulation + # joint friction coefficient + if friction_distribution_params is not None: + friction_coeff = _randomize_prop_by_op( + asset.data.default_joint_friction_coeff.clone(), + friction_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + asset.write_joint_friction_coefficient_to_sim( + friction_coeff[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids + ) + + # joint armature + if armature_distribution_params is not None: + armature = _randomize_prop_by_op( + asset.data.default_joint_armature.clone(), + armature_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + asset.write_joint_armature_to_sim(armature[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids) + + # joint position limits + if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: + joint_pos_limits = asset.data.default_joint_pos_limits.clone() + # -- randomize the lower limits + if lower_limit_distribution_params is not None: + joint_pos_limits[..., 0] = _randomize_prop_by_op( + joint_pos_limits[..., 0], + lower_limit_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + # -- randomize the upper limits + if upper_limit_distribution_params is not None: + joint_pos_limits[..., 1] = _randomize_prop_by_op( + joint_pos_limits[..., 1], + upper_limit_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + + # extract the position limits for the concerned joints + joint_pos_limits = joint_pos_limits[env_ids[:, None], joint_ids] + if (joint_pos_limits[..., 0] > joint_pos_limits[..., 1]).any(): + raise ValueError( + "Randomization term 'randomize_joint_parameters' is setting lower joint limits that are greater than" + " upper joint limits. Please check the distribution parameters for the joint position limits." + ) + # set the position limits into the physics simulation + asset.write_joint_position_limit_to_sim( + joint_pos_limits, joint_ids=joint_ids, env_ids=env_ids, warn_limit_violation=False + ) + + +def randomize_fixed_tendon_parameters( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + stiffness_distribution_params: tuple[float, float] | None = None, + damping_distribution_params: tuple[float, float] | None = None, + limit_stiffness_distribution_params: tuple[float, float] | None = None, + lower_limit_distribution_params: tuple[float, float] | None = None, + upper_limit_distribution_params: tuple[float, float] | None = None, + rest_length_distribution_params: tuple[float, float] | None = None, + offset_distribution_params: tuple[float, float] | None = None, + operation: Literal["add", "scale", "abs"] = "abs", + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", +): + """Randomize the simulated fixed tendon parameters of an articulation by adding, scaling, or setting random values. + + This function allows randomizing the fixed tendon parameters of the asset. + These correspond to the physics engine tendon properties that affect the joint behavior. + + The function samples random values from the given distribution parameters and applies the operation to the tendon properties. + It then sets the values into the physics simulation. If the distribution parameters are not provided for a + particular property, the function does not modify the property. + + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=asset.device) + + # resolve joint indices + if asset_cfg.fixed_tendon_ids == slice(None): + tendon_ids = slice(None) # for optimization purposes + else: + tendon_ids = torch.tensor(asset_cfg.fixed_tendon_ids, dtype=torch.int, device=asset.device) + + # sample tendon properties from the given ranges and set into the physics simulation + # stiffness + if stiffness_distribution_params is not None: + stiffness = _randomize_prop_by_op( + asset.data.default_fixed_tendon_stiffness.clone(), + stiffness_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + asset.set_fixed_tendon_stiffness(stiffness[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # damping + if damping_distribution_params is not None: + damping = _randomize_prop_by_op( + asset.data.default_fixed_tendon_damping.clone(), + damping_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + asset.set_fixed_tendon_damping(damping[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # limit stiffness + if limit_stiffness_distribution_params is not None: + limit_stiffness = _randomize_prop_by_op( + asset.data.default_fixed_tendon_limit_stiffness.clone(), + limit_stiffness_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + asset.set_fixed_tendon_limit_stiffness(limit_stiffness[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # position limits + if lower_limit_distribution_params is not None or upper_limit_distribution_params is not None: + limit = asset.data.default_fixed_tendon_pos_limits.clone() + # -- lower limit + if lower_limit_distribution_params is not None: + limit[..., 0] = _randomize_prop_by_op( + limit[..., 0], + lower_limit_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + # -- upper limit + if upper_limit_distribution_params is not None: + limit[..., 1] = _randomize_prop_by_op( + limit[..., 1], + upper_limit_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + + # check if the limits are valid + tendon_limits = limit[env_ids[:, None], tendon_ids] + if (tendon_limits[..., 0] > tendon_limits[..., 1]).any(): + raise ValueError( + "Randomization term 'randomize_fixed_tendon_parameters' is setting lower tendon limits that are greater" + " than upper tendon limits." + ) + asset.set_fixed_tendon_position_limit(tendon_limits, tendon_ids, env_ids) + + # rest length + if rest_length_distribution_params is not None: + rest_length = _randomize_prop_by_op( + asset.data.default_fixed_tendon_rest_length.clone(), + rest_length_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + asset.set_fixed_tendon_rest_length(rest_length[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # offset + if offset_distribution_params is not None: + offset = _randomize_prop_by_op( + asset.data.default_fixed_tendon_offset.clone(), + offset_distribution_params, + env_ids, + tendon_ids, + operation=operation, + distribution=distribution, + ) + asset.set_fixed_tendon_offset(offset[env_ids[:, None], tendon_ids], tendon_ids, env_ids) + + # write the fixed tendon properties into the simulation + asset.write_fixed_tendon_properties_to_sim(tendon_ids, env_ids) + + +def apply_external_force_torque( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + force_range: tuple[float, float], + torque_range: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Randomize the external forces and torques applied to the bodies. + + This function creates a set of random forces and torques sampled from the given ranges. The number of forces + and torques is equal to the number of bodies times the number of environments. The forces and torques are + applied to the bodies by calling ``asset.set_external_force_and_torque``. The forces and torques are only + applied when ``asset.write_data_to_sim()`` is called in the environment. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=asset.device) + # resolve number of bodies + num_bodies = len(asset_cfg.body_ids) if isinstance(asset_cfg.body_ids, list) else asset.num_bodies + + # sample random forces and torques + size = (len(env_ids), num_bodies, 3) + forces = math_utils.sample_uniform(*force_range, size, asset.device) + torques = math_utils.sample_uniform(*torque_range, size, asset.device) + # set the forces and torques into the buffers + # note: these are only applied when you call: `asset.write_data_to_sim()` + asset.set_external_force_and_torque(forces, torques, env_ids=env_ids, body_ids=asset_cfg.body_ids) + + +def push_by_setting_velocity( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + velocity_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Push the asset by setting the root velocity to a random value within the given ranges. + + This creates an effect similar to pushing the asset with a random impulse that changes the asset's velocity. + It samples the root velocity from the given ranges and sets the velocity into the physics simulation. + + The function takes a dictionary of velocity ranges for each axis and rotation. The keys of the dictionary + are ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. The values are tuples of the form ``(min, max)``. + If the dictionary does not contain a key, the velocity is set to zero for that axis. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + + # velocities + vel_w = asset.data.root_vel_w[env_ids] + # sample random velocities + range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + vel_w += math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], vel_w.shape, device=asset.device) + # set the velocities into the physics simulation + asset.write_root_velocity_to_sim(vel_w, env_ids=env_ids) + + +def reset_root_state_uniform( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + velocity_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the asset root state to a random position and velocity uniformly within the given ranges. + + This function randomizes the root position and velocity of the asset. + + * It samples the root position from the given ranges and adds them to the default root position, before setting + them into the physics simulation. + * It samples the root orientation from the given ranges and sets them into the physics simulation. + * It samples the root velocity from the given ranges and sets them into the physics simulation. + + The function takes a dictionary of pose and velocity ranges for each axis and rotation. The keys of the + dictionary are ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. The values are tuples of the form + ``(min, max)``. If the dictionary does not contain a key, the position or velocity is set to zero for that axis. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + # get default root state + root_states = asset.data.default_root_state[env_ids].clone() + + # poses + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=asset.device) + + positions = root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples[:, 0:3] + orientations_delta = math_utils.quat_from_euler_xyz(rand_samples[:, 3], rand_samples[:, 4], rand_samples[:, 5]) + orientations = math_utils.quat_mul(root_states[:, 3:7], orientations_delta) + # velocities + range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=asset.device) + + velocities = root_states[:, 7:13] + rand_samples + + # set into the physics simulation + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) + + +def reset_root_state_with_random_orientation( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + velocity_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the asset root position and velocities sampled randomly within the given ranges + and the asset root orientation sampled randomly from the SO(3). + + This function randomizes the root position and velocity of the asset. + + * It samples the root position from the given ranges and adds them to the default root position, before setting + them into the physics simulation. + * It samples the root orientation uniformly from the SO(3) and sets them into the physics simulation. + * It samples the root velocity from the given ranges and sets them into the physics simulation. + + The function takes a dictionary of position and velocity ranges for each axis and rotation: + + * :attr:`pose_range` - a dictionary of position ranges for each axis. The keys of the dictionary are ``x``, + ``y``, and ``z``. The orientation is sampled uniformly from the SO(3). + * :attr:`velocity_range` - a dictionary of velocity ranges for each axis and rotation. The keys of the dictionary + are ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. + + The values are tuples of the form ``(min, max)``. If the dictionary does not contain a particular key, + the position is set to zero for that axis. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + # get default root state + root_states = asset.data.default_root_state[env_ids].clone() + + # poses + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=asset.device) + + positions = root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_samples + orientations = math_utils.random_orientation(len(env_ids), device=asset.device) + + # velocities + range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=asset.device) + + velocities = root_states[:, 7:13] + rand_samples + + # set into the physics simulation + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) + + +def reset_root_state_from_terrain( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + velocity_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the asset root state by sampling a random valid pose from the terrain. + + This function samples a random valid pose(based on flat patches) from the terrain and sets the root state + of the asset to this position. The function also samples random velocities from the given ranges and sets them + into the physics simulation. + + The function takes a dictionary of position and velocity ranges for each axis and rotation: + + * :attr:`pose_range` - a dictionary of pose ranges for each axis. The keys of the dictionary are ``roll``, + ``pitch``, and ``yaw``. The position is sampled from the flat patches of the terrain. + * :attr:`velocity_range` - a dictionary of velocity ranges for each axis and rotation. The keys of the dictionary + are ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. + + The values are tuples of the form ``(min, max)``. If the dictionary does not contain a particular key, + the position is set to zero for that axis. + + Note: + The function expects the terrain to have valid flat patches under the key "init_pos". The flat patches + are used to sample the random pose for the robot. + + Raises: + ValueError: If the terrain does not have valid flat patches under the key "init_pos". + """ + # access the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + terrain: TerrainImporter = env.scene.terrain + + # obtain all flat patches corresponding to the valid poses + valid_positions: torch.Tensor = terrain.flat_patches.get("init_pos") + if valid_positions is None: + raise ValueError( + "The event term 'reset_root_state_from_terrain' requires valid flat patches under 'init_pos'." + f" Found: {list(terrain.flat_patches.keys())}" + ) + + # sample random valid poses + ids = torch.randint(0, valid_positions.shape[2], size=(len(env_ids),), device=env.device) + positions = valid_positions[terrain.terrain_levels[env_ids], terrain.terrain_types[env_ids], ids] + positions += asset.data.default_root_state[env_ids, :3] + + # sample random orientations + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=asset.device) + + # convert to quaternions + orientations = math_utils.quat_from_euler_xyz(rand_samples[:, 0], rand_samples[:, 1], rand_samples[:, 2]) + + # sample random velocities + range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=asset.device) + + velocities = asset.data.default_root_state[env_ids, 7:13] + rand_samples + + # set into the physics simulation + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) + + +def reset_joints_by_scale( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: tuple[float, float], + velocity_range: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the robot joints by scaling the default position and velocity by the given ranges. + + This function samples random values from the given ranges and scales the default joint positions and velocities + by these values. The scaled values are then set into the physics simulation. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # get default joint state + joint_pos = asset.data.default_joint_pos[env_ids].clone() + joint_vel = asset.data.default_joint_vel[env_ids].clone() + + # scale these values randomly + joint_pos *= math_utils.sample_uniform(*position_range, joint_pos.shape, joint_pos.device) + joint_vel *= math_utils.sample_uniform(*velocity_range, joint_vel.shape, joint_vel.device) + + # clamp joint pos to limits + joint_pos_limits = asset.data.soft_joint_pos_limits[env_ids] + joint_pos = joint_pos.clamp_(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) + # clamp joint vel to limits + joint_vel_limits = asset.data.soft_joint_vel_limits[env_ids] + joint_vel = joint_vel.clamp_(-joint_vel_limits, joint_vel_limits) + + # set into the physics simulation + asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + +def reset_joints_by_offset( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: tuple[float, float], + velocity_range: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the robot joints with offsets around the default position and velocity by the given ranges. + + This function samples random values from the given ranges and biases the default joint positions and velocities + by these values. The biased values are then set into the physics simulation. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # get default joint state + joint_pos = asset.data.default_joint_pos[env_ids].clone() + joint_vel = asset.data.default_joint_vel[env_ids].clone() + + # bias these values randomly + joint_pos += math_utils.sample_uniform(*position_range, joint_pos.shape, joint_pos.device) + joint_vel += math_utils.sample_uniform(*velocity_range, joint_vel.shape, joint_vel.device) + + # clamp joint pos to limits + joint_pos_limits = asset.data.soft_joint_pos_limits[env_ids] + joint_pos = joint_pos.clamp_(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) + # clamp joint vel to limits + joint_vel_limits = asset.data.soft_joint_vel_limits[env_ids] + joint_vel = joint_vel.clamp_(-joint_vel_limits, joint_vel_limits) + + # set into the physics simulation + asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + +def reset_nodal_state_uniform( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: dict[str, tuple[float, float]], + velocity_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the asset nodal state to a random position and velocity uniformly within the given ranges. + + This function randomizes the nodal position and velocity of the asset. + + * It samples the root position from the given ranges and adds them to the default nodal position, before setting + them into the physics simulation. + * It samples the root velocity from the given ranges and sets them into the physics simulation. + + The function takes a dictionary of position and velocity ranges for each axis. The keys of the + dictionary are ``x``, ``y``, ``z``. The values are tuples of the form ``(min, max)``. + If the dictionary does not contain a key, the position or velocity is set to zero for that axis. + """ + # extract the used quantities (to enable type-hinting) + asset: DeformableObject = env.scene[asset_cfg.name] + # get default root state + nodal_state = asset.data.default_nodal_state_w[env_ids].clone() + + # position + range_list = [position_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 1, 3), device=asset.device) + + nodal_state[..., :3] += rand_samples + + # velocities + range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=asset.device) + rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 1, 3), device=asset.device) + + nodal_state[..., 3:] += rand_samples + + # set into the physics simulation + asset.write_nodal_state_to_sim(nodal_state, env_ids=env_ids) + + +def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor): + """Reset the scene to the default state specified in the scene configuration.""" + # rigid bodies + for rigid_object in env.scene.rigid_objects.values(): + # obtain default and deal with the offset for env origins + default_root_state = rigid_object.data.default_root_state[env_ids].clone() + default_root_state[:, 0:3] += env.scene.env_origins[env_ids] + # set into the physics simulation + rigid_object.write_root_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) + rigid_object.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) + # articulations + for articulation_asset in env.scene.articulations.values(): + # obtain default and deal with the offset for env origins + default_root_state = articulation_asset.data.default_root_state[env_ids].clone() + default_root_state[:, 0:3] += env.scene.env_origins[env_ids] + # set into the physics simulation + articulation_asset.write_root_pose_to_sim(default_root_state[:, :7], env_ids=env_ids) + articulation_asset.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids=env_ids) + # obtain default joint positions + default_joint_pos = articulation_asset.data.default_joint_pos[env_ids].clone() + default_joint_vel = articulation_asset.data.default_joint_vel[env_ids].clone() + # set into the physics simulation + articulation_asset.write_joint_state_to_sim(default_joint_pos, default_joint_vel, env_ids=env_ids) + # deformable objects + for deformable_object in env.scene.deformable_objects.values(): + # obtain default and set into the physics simulation + nodal_state = deformable_object.data.default_nodal_state_w[env_ids].clone() + deformable_object.write_nodal_state_to_sim(nodal_state, env_ids=env_ids) + + +class randomize_visual_texture_material(ManagerTermBase): + """Randomize the visual texture of bodies on an asset using Replicator API. + + This function randomizes the visual texture of the bodies of the asset using the Replicator API. + The function samples random textures from the given texture paths and applies them to the bodies + of the asset. The textures are projected onto the bodies and rotated by the given angles. + + .. note:: + The function assumes that the asset follows the prim naming convention as: + "{asset_prim_path}/{body_name}/visuals" where the body name is the name of the body to + which the texture is applied. This is the default prim ordering when importing assets + from the asset converters in Isaac Lab. + + .. note:: + When randomizing the texture of individual assets, please make sure to set + :attr:`isaaclab.scene.InteractiveSceneCfg.replicate_physics` to False. This ensures that physics + parser will parse the individual asset properties separately. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + """ + super().__init__(cfg, env) + + # enable replicator extension if not already enabled + enable_extension("omni.replicator.core") + # we import the module here since we may not always need the replicator + import omni.replicator.core as rep + + # read parameters from the configuration + asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg") + texture_paths = cfg.params.get("texture_paths") + event_name = cfg.params.get("event_name") + texture_rotation = cfg.params.get("texture_rotation", (0.0, 0.0)) + + # check to make sure replicate_physics is set to False, else raise error + # note: We add an explicit check here since texture randomization can happen outside of 'prestartup' mode + # and the event manager doesn't check in that case. + if env.cfg.scene.replicate_physics: + raise RuntimeError( + "Unable to randomize visual texture material with scene replication enabled." + " For stable USD-level randomization, please disable scene replication" + " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." + ) + + # convert from radians to degrees + texture_rotation = tuple(math.degrees(angle) for angle in texture_rotation) + + # obtain the asset entity + asset = env.scene[asset_cfg.name] + + # join all bodies in the asset + body_names = asset_cfg.body_names + if isinstance(body_names, str): + body_names_regex = body_names + elif isinstance(body_names, list): + body_names_regex = "|".join(body_names) + else: + body_names_regex = ".*" + + # create the affected prim path + # TODO: Remove the hard-coded "/visuals" part. + prim_path = f"{asset.cfg.prim_path}/{body_names_regex}/visuals" + + # Create the omni-graph node for the randomization term + def rep_texture_randomization(): + prims_group = rep.get.prims(path_pattern=prim_path) + + with prims_group: + rep.randomizer.texture( + textures=texture_paths, project_uvw=True, texture_rotate=rep.distribution.uniform(*texture_rotation) + ) + + return prims_group.node + + # Register the event to the replicator + with rep.trigger.on_custom_event(event_name=event_name): + rep_texture_randomization() + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + event_name: str, + asset_cfg: SceneEntityCfg, + texture_paths: list[str], + texture_rotation: tuple[float, float] = (0.0, 0.0), + ): + # import replicator + import omni.replicator.core as rep + + # only send the event to the replicator + # note: This triggers the nodes for all the environments. + # We need to investigate how to make it happen only for a subset based on env_ids. + rep.utils.send_og_event(event_name) + + +class randomize_visual_color(ManagerTermBase): + """Randomize the visual color of bodies on an asset using Replicator API. + + This function randomizes the visual color of the bodies of the asset using the Replicator API. + The function samples random colors from the given colors and applies them to the bodies + of the asset. + + The function assumes that the asset follows the prim naming convention as: + "{asset_prim_path}/{mesh_name}" where the mesh name is the name of the mesh to + which the color is applied. For instance, if the asset has a prim path "/World/asset" + and a mesh named "body_0/mesh", the prim path for the mesh would be + "/World/asset/body_0/mesh". + + The colors can be specified as a list of tuples of the form ``(r, g, b)`` or as a dictionary + with the keys ``r``, ``g``, ``b`` and values as tuples of the form ``(low, high)``. + If a dictionary is used, the function will sample random colors from the given ranges. + + .. note:: + When randomizing the color of individual assets, please make sure to set + :attr:`isaaclab.scene.InteractiveSceneCfg.replicate_physics` to False. This ensures that physics + parser will parse the individual asset properties separately. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the randomization term.""" + super().__init__(cfg, env) + + # enable replicator extension if not already enabled + enable_extension("omni.replicator.core") + # we import the module here since we may not always need the replicator + import omni.replicator.core as rep + + # read parameters from the configuration + asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg") + colors = cfg.params.get("colors") + event_name = cfg.params.get("event_name") + mesh_name: str = cfg.params.get("mesh_name", "") # type: ignore + + # check to make sure replicate_physics is set to False, else raise error + # note: We add an explicit check here since texture randomization can happen outside of 'prestartup' mode + # and the event manager doesn't check in that case. + if env.cfg.scene.replicate_physics: + raise RuntimeError( + "Unable to randomize visual color with scene replication enabled." + " For stable USD-level randomization, please disable scene replication" + " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." + ) + + # obtain the asset entity + asset = env.scene[asset_cfg.name] + + # create the affected prim path + if not mesh_name.startswith("/"): + mesh_name = "/" + mesh_name + mesh_prim_path = f"{asset.cfg.prim_path}{mesh_name}" + # TODO: Need to make it work for multiple meshes. + + # parse the colors into replicator format + if isinstance(colors, dict): + # (r, g, b) - low, high --> (low_r, low_g, low_b) and (high_r, high_g, high_b) + color_low = [colors[key][0] for key in ["r", "g", "b"]] + color_high = [colors[key][1] for key in ["r", "g", "b"]] + colors = rep.distribution.uniform(color_low, color_high) + else: + colors = list(colors) + + # Create the omni-graph node for the randomization term + def rep_texture_randomization(): + prims_group = rep.get.prims(path_pattern=mesh_prim_path) + + with prims_group: + rep.randomizer.color(colors=colors) + + return prims_group.node + + # Register the event to the replicator + with rep.trigger.on_custom_event(event_name=event_name): + rep_texture_randomization() + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + event_name: str, + asset_cfg: SceneEntityCfg, + colors: list[tuple[float, float, float]] | dict[str, tuple[float, float]], + mesh_name: str = "", + ): + # import replicator + import omni.replicator.core as rep + + # only send the event to the replicator + rep.utils.send_og_event(event_name) + + +""" +Internal helper functions. +""" + + +def _randomize_prop_by_op( + data: torch.Tensor, + distribution_parameters: tuple[float | torch.Tensor, float | torch.Tensor], + dim_0_ids: torch.Tensor | None, + dim_1_ids: torch.Tensor | slice, + operation: Literal["add", "scale", "abs"], + distribution: Literal["uniform", "log_uniform", "gaussian"], +) -> torch.Tensor: + """Perform data randomization based on the given operation and distribution. + + Args: + data: The data tensor to be randomized. Shape is (dim_0, dim_1). + distribution_parameters: The parameters for the distribution to sample values from. + dim_0_ids: The indices of the first dimension to randomize. + dim_1_ids: The indices of the second dimension to randomize. + operation: The operation to perform on the data. Options: 'add', 'scale', 'abs'. + distribution: The distribution to sample the random values from. Options: 'uniform', 'log_uniform'. + + Returns: + The data tensor after randomization. Shape is (dim_0, dim_1). + + Raises: + NotImplementedError: If the operation or distribution is not supported. + """ + # resolve shape + # -- dim 0 + if dim_0_ids is None: + n_dim_0 = data.shape[0] + dim_0_ids = slice(None) + else: + n_dim_0 = len(dim_0_ids) + if not isinstance(dim_1_ids, slice): + dim_0_ids = dim_0_ids[:, None] + # -- dim 1 + if isinstance(dim_1_ids, slice): + n_dim_1 = data.shape[1] + else: + n_dim_1 = len(dim_1_ids) + + # resolve the distribution + if distribution == "uniform": + dist_fn = math_utils.sample_uniform + elif distribution == "log_uniform": + dist_fn = math_utils.sample_log_uniform + elif distribution == "gaussian": + dist_fn = math_utils.sample_gaussian + else: + raise NotImplementedError( + f"Unknown distribution: '{distribution}' for joint properties randomization." + " Please use 'uniform', 'log_uniform', 'gaussian'." + ) + # perform the operation + if operation == "add": + data[dim_0_ids, dim_1_ids] += dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device) + elif operation == "scale": + data[dim_0_ids, dim_1_ids] *= dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device) + elif operation == "abs": + data[dim_0_ids, dim_1_ids] = dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device) + else: + raise NotImplementedError( + f"Unknown operation: '{operation}' for property randomization. Please use 'add', 'scale', or 'abs'." + ) + return data diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/observations.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/observations.py new file mode 100644 index 00000000000..c28725072f5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/observations.py @@ -0,0 +1,620 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create observation terms. + +The functions can be passed to the :class:`isaaclab.managers.ObservationTermCfg` object to enable +the observation introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers.manager_base import ManagerTermBase +from isaaclab.managers.manager_term_cfg import ObservationTermCfg +from isaaclab.sensors import Camera, Imu, RayCaster, RayCasterCamera, TiledCamera + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv + + +""" +Root state. +""" + + +def base_pos_z(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Root height in the simulation world frame.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.root_pos_w[:, 2].unsqueeze(-1) + + +def base_lin_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Root linear velocity in the asset's root frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_lin_vel_b + + +def base_ang_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Root angular velocity in the asset's root frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_ang_vel_b + + +def projected_gravity(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Gravity projection on the asset's root frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.projected_gravity_b + + +def root_pos_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Asset root position in the environment frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_pos_w - env.scene.env_origins + + +def root_quat_w( + env: ManagerBasedEnv, make_quat_unique: bool = False, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Asset root orientation (w, x, y, z) in the environment frame. + + If :attr:`make_quat_unique` is True, then returned quaternion is made unique by ensuring + the quaternion has non-negative real component. This is because both ``q`` and ``-q`` represent + the same orientation. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + quat = asset.data.root_quat_w + # make the quaternion real-part positive if configured + return math_utils.quat_unique(quat) if make_quat_unique else quat + + +def root_lin_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Asset root linear velocity in the environment frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_lin_vel_w + + +def root_ang_vel_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Asset root angular velocity in the environment frame.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_ang_vel_w + + +""" +Body state +""" + + +def body_pose_w( + env: ManagerBasedEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """The flattened body poses of the asset w.r.t the env.scene.origin. + + Note: Only the bodies configured in :attr:`asset_cfg.body_ids` will have their poses returned. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with this observation. + + Returns: + The poses of bodies in articulation [num_env, 7*num_bodies]. Pose order is [x,y,z,qw,qx,qy,qz]. Output is + stacked horizontally per body. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + pose = asset.data.body_state_w[:, asset_cfg.body_ids, :7] + pose[..., :3] = pose[..., :3] - env.scene.env_origins.unsqueeze(1) + return pose.reshape(env.num_envs, -1) + + +def body_projected_gravity_b( + env: ManagerBasedEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """The direction of gravity projected on to bodies of an Articulation. + + Note: Only the bodies configured in :attr:`asset_cfg.body_ids` will have their poses returned. + + Args: + env: The environment. + asset_cfg: The Articulation associated with this observation. + + Returns: + The unit vector direction of gravity projected onto body_name's frame. Gravity projection vector order is + [x,y,z]. Output is stacked horizontally per body. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + body_quat = asset.data.body_quat_w[:, asset_cfg.body_ids] + gravity_dir = asset.data.GRAVITY_VEC_W.unsqueeze(1) + return math_utils.quat_apply_inverse(body_quat, gravity_dir).view(env.num_envs, -1) + + +""" +Joint state. +""" + + +def joint_pos(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """The joint positions of the asset. + + Note: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their positions returned. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.joint_pos[:, asset_cfg.joint_ids] + + +def joint_pos_rel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """The joint positions of the asset w.r.t. the default joint positions. + + Note: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their positions returned. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids] + + +def joint_pos_limit_normalized( + env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """The joint positions of the asset normalized with the asset's joint limits. + + Note: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their normalized positions returned. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return math_utils.scale_transform( + asset.data.joint_pos[:, asset_cfg.joint_ids], + asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 0], + asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 1], + ) + + +def joint_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")): + """The joint velocities of the asset. + + Note: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their velocities returned. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.joint_vel[:, asset_cfg.joint_ids] + + +def joint_vel_rel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")): + """The joint velocities of the asset w.r.t. the default joint velocities. + + Note: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their velocities returned. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.joint_vel[:, asset_cfg.joint_ids] - asset.data.default_joint_vel[:, asset_cfg.joint_ids] + + +def joint_effort(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """The joint applied effort of the robot. + + NOTE: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their effort returned. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with this observation. + + Returns: + The joint effort (N or N-m) for joint_names in asset_cfg, shape is [num_env,num_joints]. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.applied_torque[:, asset_cfg.joint_ids] + + +""" +Sensors. +""" + + +def height_scan(env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg, offset: float = 0.5) -> torch.Tensor: + """Height scan from the given sensor w.r.t. the sensor's frame. + + The provided offset (Defaults to 0.5) is subtracted from the returned values. + """ + # extract the used quantities (to enable type-hinting) + sensor: RayCaster = env.scene.sensors[sensor_cfg.name] + # height scan: height = sensor_height - hit_point_z - offset + return sensor.data.pos_w[:, 2].unsqueeze(1) - sensor.data.ray_hits_w[..., 2] - offset + + +def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Incoming spatial wrench on bodies of an articulation in the simulation world frame. + + This is the 6-D wrench (force and torque) applied to the body link by the incoming joint force. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # obtain the link incoming forces in world frame + body_incoming_joint_wrench_b = asset.data.body_incoming_joint_wrench_b[:, asset_cfg.body_ids] + return body_incoming_joint_wrench_b.view(env.num_envs, -1) + + +def imu_orientation(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor: + """Imu sensor orientation in the simulation world frame. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with an IMU sensor. Defaults to SceneEntityCfg("imu"). + + Returns: + Orientation in the world frame in (w, x, y, z) quaternion form. Shape is (num_envs, 4). + """ + # extract the used quantities (to enable type-hinting) + asset: Imu = env.scene[asset_cfg.name] + # return the orientation quaternion + return asset.data.quat_w + + +def imu_ang_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor: + """Imu sensor angular velocity w.r.t. environment origin expressed in the sensor frame. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with an IMU sensor. Defaults to SceneEntityCfg("imu"). + + Returns: + The angular velocity (rad/s) in the sensor frame. Shape is (num_envs, 3). + """ + # extract the used quantities (to enable type-hinting) + asset: Imu = env.scene[asset_cfg.name] + # return the angular velocity + return asset.data.ang_vel_b + + +def imu_lin_acc(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor: + """Imu sensor linear acceleration w.r.t. the environment origin expressed in sensor frame. + + Args: + env: The environment. + asset_cfg: The SceneEntity associated with an IMU sensor. Defaults to SceneEntityCfg("imu"). + + Returns: + The linear acceleration (m/s^2) in the sensor frame. Shape is (num_envs, 3). + """ + asset: Imu = env.scene[asset_cfg.name] + return asset.data.lin_acc_b + + +def image( + env: ManagerBasedEnv, + sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"), + data_type: str = "rgb", + convert_perspective_to_orthogonal: bool = False, + normalize: bool = True, +) -> torch.Tensor: + """Images of a specific datatype from the camera sensor. + + If the flag :attr:`normalize` is True, post-processing of the images are performed based on their + data-types: + + - "rgb": Scales the image to (0, 1) and subtracts with the mean of the current image batch. + - "depth" or "distance_to_camera" or "distance_to_plane": Replaces infinity values with zero. + + Args: + env: The environment the cameras are placed within. + sensor_cfg: The desired sensor to read from. Defaults to SceneEntityCfg("tiled_camera"). + data_type: The data type to pull from the desired camera. Defaults to "rgb". + convert_perspective_to_orthogonal: Whether to orthogonalize perspective depth images. + This is used only when the data type is "distance_to_camera". Defaults to False. + normalize: Whether to normalize the images. This depends on the selected data type. + Defaults to True. + + Returns: + The images produced at the last time-step + """ + # extract the used quantities (to enable type-hinting) + sensor: TiledCamera | Camera | RayCasterCamera = env.scene.sensors[sensor_cfg.name] + + # obtain the input image + images = sensor.data.output[data_type] + + # depth image conversion + if (data_type == "distance_to_camera") and convert_perspective_to_orthogonal: + images = math_utils.orthogonalize_perspective_depth(images, sensor.data.intrinsic_matrices) + + # rgb/depth image normalization + if normalize: + if data_type == "rgb": + images = images.float() / 255.0 + mean_tensor = torch.mean(images, dim=(1, 2), keepdim=True) + images -= mean_tensor + elif "distance_to" in data_type or "depth" in data_type: + images[images == float("inf")] = 0 + + return images.clone() + + +class image_features(ManagerTermBase): + """Extracted image features from a pre-trained frozen encoder. + + This term uses models from the model zoo in PyTorch and extracts features from the images. + + It calls the :func:`image` function to get the images and then processes them using the model zoo. + + A user can provide their own model zoo configuration to use different models for feature extraction. + The model zoo configuration should be a dictionary that maps different model names to a dictionary + that defines the model, preprocess and inference functions. The dictionary should have the following + entries: + + - "model": A callable that returns the model when invoked without arguments. + - "reset": A callable that resets the model. This is useful when the model has a state that needs to be reset. + - "inference": A callable that, when given the model and the images, returns the extracted features. + + If the model zoo configuration is not provided, the default model zoo configurations are used. The default + model zoo configurations include the models from Theia :cite:`shang2024theia` and ResNet :cite:`he2016deep`. + These models are loaded from `Hugging-Face transformers `_ and + `PyTorch torchvision `_ respectively. + + Args: + sensor_cfg: The sensor configuration to poll. Defaults to SceneEntityCfg("tiled_camera"). + data_type: The sensor data type. Defaults to "rgb". + convert_perspective_to_orthogonal: Whether to orthogonalize perspective depth images. + This is used only when the data type is "distance_to_camera". Defaults to False. + model_zoo_cfg: A user-defined dictionary that maps different model names to their respective configurations. + Defaults to None. If None, the default model zoo configurations are used. + model_name: The name of the model to use for inference. Defaults to "resnet18". + model_device: The device to store and infer the model on. This is useful when offloading the computation + from the environment simulation device. Defaults to the environment device. + inference_kwargs: Additional keyword arguments to pass to the inference function. Defaults to None, + which means no additional arguments are passed. + + Returns: + The extracted features tensor. Shape is (num_envs, feature_dim). + + Raises: + ValueError: When the model name is not found in the provided model zoo configuration. + ValueError: When the model name is not found in the default model zoo configuration. + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedEnv): + # initialize the base class + super().__init__(cfg, env) + + # extract parameters from the configuration + self.model_zoo_cfg: dict = cfg.params.get("model_zoo_cfg") # type: ignore + self.model_name: str = cfg.params.get("model_name", "resnet18") # type: ignore + self.model_device: str = cfg.params.get("model_device", env.device) # type: ignore + + # List of Theia models - These are configured through `_prepare_theia_transformer_model` function + default_theia_models = [ + "theia-tiny-patch16-224-cddsv", + "theia-tiny-patch16-224-cdiv", + "theia-small-patch16-224-cdiv", + "theia-base-patch16-224-cdiv", + "theia-small-patch16-224-cddsv", + "theia-base-patch16-224-cddsv", + ] + # List of ResNet models - These are configured through `_prepare_resnet_model` function + default_resnet_models = ["resnet18", "resnet34", "resnet50", "resnet101"] + + # Check if model name is specified in the model zoo configuration + if self.model_zoo_cfg is not None and self.model_name not in self.model_zoo_cfg: + raise ValueError( + f"Model name '{self.model_name}' not found in the provided model zoo configuration." + " Please add the model to the model zoo configuration or use a different model name." + f" Available models in the provided list: {list(self.model_zoo_cfg.keys())}." + "\nHint: If you want to use a default model, consider using one of the following models:" + f" {default_theia_models + default_resnet_models}. In this case, you can remove the" + " 'model_zoo_cfg' parameter from the observation term configuration." + ) + if self.model_zoo_cfg is None: + if self.model_name in default_theia_models: + model_config = self._prepare_theia_transformer_model(self.model_name, self.model_device) + elif self.model_name in default_resnet_models: + model_config = self._prepare_resnet_model(self.model_name, self.model_device) + else: + raise ValueError( + f"Model name '{self.model_name}' not found in the default model zoo configuration." + f" Available models: {default_theia_models + default_resnet_models}." + ) + else: + model_config = self.model_zoo_cfg[self.model_name] + + # Retrieve the model, preprocess and inference functions + self._model = model_config["model"]() + self._reset_fn = model_config.get("reset") + self._inference_fn = model_config["inference"] + + def reset(self, env_ids: torch.Tensor | None = None): + # reset the model if a reset function is provided + # this might be useful when the model has a state that needs to be reset + # for example: video transformers + if self._reset_fn is not None: + self._reset_fn(self._model, env_ids) + + def __call__( + self, + env: ManagerBasedEnv, + sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"), + data_type: str = "rgb", + convert_perspective_to_orthogonal: bool = False, + model_zoo_cfg: dict | None = None, + model_name: str = "resnet18", + model_device: str | None = None, + inference_kwargs: dict | None = None, + ) -> torch.Tensor: + # obtain the images from the sensor + image_data = image( + env=env, + sensor_cfg=sensor_cfg, + data_type=data_type, + convert_perspective_to_orthogonal=convert_perspective_to_orthogonal, + normalize=False, # we pre-process based on model + ) + # store the device of the image + image_device = image_data.device + # forward the images through the model + features = self._inference_fn(self._model, image_data, **(inference_kwargs or {})) + + # move the features back to the image device + return features.detach().to(image_device) + + """ + Helper functions. + """ + + def _prepare_theia_transformer_model(self, model_name: str, model_device: str) -> dict: + """Prepare the Theia transformer model for inference. + + Args: + model_name: The name of the Theia transformer model to prepare. + model_device: The device to store and infer the model on. + + Returns: + A dictionary containing the model and inference functions. + """ + from transformers import AutoModel + + def _load_model() -> torch.nn.Module: + """Load the Theia transformer model.""" + model = AutoModel.from_pretrained(f"theaiinstitute/{model_name}", trust_remote_code=True).eval() + return model.to(model_device) + + def _inference(model, images: torch.Tensor) -> torch.Tensor: + """Inference the Theia transformer model. + + Args: + model: The Theia transformer model. + images: The preprocessed image tensor. Shape is (num_envs, height, width, channel). + + Returns: + The extracted features tensor. Shape is (num_envs, feature_dim). + """ + # Move the image to the model device + image_proc = images.to(model_device) + # permute the image to (num_envs, channel, height, width) + image_proc = image_proc.permute(0, 3, 1, 2).float() / 255.0 + # Normalize the image + mean = torch.tensor([0.485, 0.456, 0.406], device=model_device).view(1, 3, 1, 1) + std = torch.tensor([0.229, 0.224, 0.225], device=model_device).view(1, 3, 1, 1) + image_proc = (image_proc - mean) / std + + # Taken from Transformers; inference converted to be GPU only + features = model.backbone.model(pixel_values=image_proc, interpolate_pos_encoding=True) + return features.last_hidden_state[:, 1:] + + # return the model, preprocess and inference functions + return {"model": _load_model, "inference": _inference} + + def _prepare_resnet_model(self, model_name: str, model_device: str) -> dict: + """Prepare the ResNet model for inference. + + Args: + model_name: The name of the ResNet model to prepare. + model_device: The device to store and infer the model on. + + Returns: + A dictionary containing the model and inference functions. + """ + from torchvision import models + + def _load_model() -> torch.nn.Module: + """Load the ResNet model.""" + # map the model name to the weights + resnet_weights = { + "resnet18": "ResNet18_Weights.IMAGENET1K_V1", + "resnet34": "ResNet34_Weights.IMAGENET1K_V1", + "resnet50": "ResNet50_Weights.IMAGENET1K_V1", + "resnet101": "ResNet101_Weights.IMAGENET1K_V1", + } + + # load the model + model = getattr(models, model_name)(weights=resnet_weights[model_name]).eval() + return model.to(model_device) + + def _inference(model, images: torch.Tensor) -> torch.Tensor: + """Inference the ResNet model. + + Args: + model: The ResNet model. + images: The preprocessed image tensor. Shape is (num_envs, channel, height, width). + + Returns: + The extracted features tensor. Shape is (num_envs, feature_dim). + """ + # move the image to the model device + image_proc = images.to(model_device) + # permute the image to (num_envs, channel, height, width) + image_proc = image_proc.permute(0, 3, 1, 2).float() / 255.0 + # normalize the image + mean = torch.tensor([0.485, 0.456, 0.406], device=model_device).view(1, 3, 1, 1) + std = torch.tensor([0.229, 0.224, 0.225], device=model_device).view(1, 3, 1, 1) + image_proc = (image_proc - mean) / std + + # forward the image through the model + return model(image_proc) + + # return the model, preprocess and inference functions + return {"model": _load_model, "inference": _inference} + + +""" +Actions. +""" + + +def last_action(env: ManagerBasedEnv, action_name: str | None = None) -> torch.Tensor: + """The last input action to the environment. + + The name of the action term for which the action is required. If None, the + entire action tensor is returned. + """ + if action_name is None: + return env.action_manager.action + else: + return env.action_manager.get_term(action_name).raw_actions + + +""" +Commands. +""" + + +def generated_commands(env: ManagerBasedRLEnv, command_name: str) -> torch.Tensor: + """The generated command from command term in the command manager with the given name.""" + return env.command_manager.get_command(command_name) + + +""" +Time. +""" + + +def current_time_s(env: ManagerBasedRLEnv) -> torch.Tensor: + """The current time in the episode (in seconds).""" + return env.episode_length_buf.unsqueeze(1) * env.step_dt + + +def remaining_time_s(env: ManagerBasedRLEnv) -> torch.Tensor: + """The maximum time remaining in the episode (in seconds).""" + return env.max_episode_length_s - env.episode_length_buf.unsqueeze(1) * env.step_dt diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/__init__.py new file mode 100644 index 00000000000..74df7664912 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Various recorder terms that can be used in the environment.""" + +from .recorders import * +from .recorders_cfg import * diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders.py new file mode 100644 index 00000000000..713d95d0ed5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Sequence + +from isaaclab.managers.recorder_manager import RecorderTerm + + +class InitialStateRecorder(RecorderTerm): + """Recorder term that records the initial state of the environment after reset.""" + + def record_post_reset(self, env_ids: Sequence[int] | None): + def extract_env_ids_values(value): + nonlocal env_ids + if isinstance(value, dict): + return {k: extract_env_ids_values(v) for k, v in value.items()} + return value[env_ids] + + return "initial_state", extract_env_ids_values(self._env.scene.get_state(is_relative=True)) + + +class PostStepStatesRecorder(RecorderTerm): + """Recorder term that records the state of the environment at the end of each step.""" + + def record_post_step(self): + return "states", self._env.scene.get_state(is_relative=True) + + +class PreStepActionsRecorder(RecorderTerm): + """Recorder term that records the actions in the beginning of each step.""" + + def record_pre_step(self): + return "actions", self._env.action_manager.action + + +class PreStepFlatPolicyObservationsRecorder(RecorderTerm): + """Recorder term that records the policy group observations in each step.""" + + def record_pre_step(self): + return "obs", self._env.obs_buf["policy"] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders_cfg.py new file mode 100644 index 00000000000..bbd19e70e3a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/recorders/recorders_cfg.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg, RecorderTerm, RecorderTermCfg +from isaaclab.utils import configclass + +from . import recorders + +## +# State recorders. +## + + +@configclass +class InitialStateRecorderCfg(RecorderTermCfg): + """Configuration for the initial state recorder term.""" + + class_type: type[RecorderTerm] = recorders.InitialStateRecorder + + +@configclass +class PostStepStatesRecorderCfg(RecorderTermCfg): + """Configuration for the step state recorder term.""" + + class_type: type[RecorderTerm] = recorders.PostStepStatesRecorder + + +@configclass +class PreStepActionsRecorderCfg(RecorderTermCfg): + """Configuration for the step action recorder term.""" + + class_type: type[RecorderTerm] = recorders.PreStepActionsRecorder + + +@configclass +class PreStepFlatPolicyObservationsRecorderCfg(RecorderTermCfg): + """Configuration for the step policy observation recorder term.""" + + class_type: type[RecorderTerm] = recorders.PreStepFlatPolicyObservationsRecorder + + +## +# Recorder manager configurations. +## + + +@configclass +class ActionStateRecorderManagerCfg(RecorderManagerBaseCfg): + """Recorder configurations for recording actions and states.""" + + record_initial_state = InitialStateRecorderCfg() + record_post_step_states = PostStepStatesRecorderCfg() + record_pre_step_actions = PreStepActionsRecorderCfg() + record_pre_step_flat_policy_observations = PreStepFlatPolicyObservationsRecorderCfg() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/rewards.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/rewards.py new file mode 100644 index 00000000000..de91e97ef2b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/rewards.py @@ -0,0 +1,314 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to enable reward functions. + +The functions can be passed to the :class:`isaaclab.managers.RewardTermCfg` object to include +the reward introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers.manager_base import ManagerTermBase +from isaaclab.managers.manager_term_cfg import RewardTermCfg +from isaaclab.sensors import ContactSensor, RayCaster + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +""" +General. +""" + + +def is_alive(env: ManagerBasedRLEnv) -> torch.Tensor: + """Reward for being alive.""" + return (~env.termination_manager.terminated).float() + + +def is_terminated(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize terminated episodes that don't correspond to episodic timeouts.""" + return env.termination_manager.terminated.float() + + +class is_terminated_term(ManagerTermBase): + """Penalize termination for specific terms that don't correspond to episodic timeouts. + + The parameters are as follows: + + * attr:`term_keys`: The termination terms to penalize. This can be a string, a list of strings + or regular expressions. Default is ".*" which penalizes all terminations. + + The reward is computed as the sum of the termination terms that are not episodic timeouts. + This means that the reward is 0 if the episode is terminated due to an episodic timeout. Otherwise, + if two termination terms are active, the reward is 2. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + # initialize the base class + super().__init__(cfg, env) + # find and store the termination terms + term_keys = cfg.params.get("term_keys", ".*") + self._term_names = env.termination_manager.find_terms(term_keys) + + def __call__(self, env: ManagerBasedRLEnv, term_keys: str | list[str] = ".*") -> torch.Tensor: + # Return the unweighted reward for the termination terms + reset_buf = torch.zeros(env.num_envs, device=env.device) + for term in self._term_names: + # Sums over terminations term values to account for multiple terminations in the same step + reset_buf += env.termination_manager.get_term(term) + + return (reset_buf * (~env.termination_manager.time_outs)).float() + + +""" +Root penalties. +""" + + +def lin_vel_z_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize z-axis base linear velocity using L2 squared kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return torch.square(asset.data.root_lin_vel_b[:, 2]) + + +def ang_vel_xy_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize xy-axis base angular velocity using L2 squared kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return torch.sum(torch.square(asset.data.root_ang_vel_b[:, :2]), dim=1) + + +def flat_orientation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize non-flat base orientation using L2 squared kernel. + + This is computed by penalizing the xy-components of the projected gravity vector. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return torch.sum(torch.square(asset.data.projected_gravity_b[:, :2]), dim=1) + + +def base_height_l2( + env: ManagerBasedRLEnv, + target_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg | None = None, +) -> torch.Tensor: + """Penalize asset height from its target using L2 squared kernel. + + Note: + For flat terrain, target height is in the world frame. For rough terrain, + sensor readings can adjust the target height to account for the terrain. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + if sensor_cfg is not None: + sensor: RayCaster = env.scene[sensor_cfg.name] + # Adjust the target height using the sensor data + adjusted_target_height = target_height + torch.mean(sensor.data.ray_hits_w[..., 2], dim=1) + else: + # Use the provided target height directly for flat terrain + adjusted_target_height = target_height + # Compute the L2 squared penalty + return torch.square(asset.data.root_pos_w[:, 2] - adjusted_target_height) + + +def body_lin_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize the linear acceleration of bodies using L2-kernel.""" + asset: Articulation = env.scene[asset_cfg.name] + return torch.sum(torch.norm(asset.data.body_lin_acc_w[:, asset_cfg.body_ids, :], dim=-1), dim=1) + + +""" +Joint penalties. +""" + + +def joint_torques_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize joint torques applied on the articulation using L2 squared kernel. + + NOTE: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their joint torques contribute to the term. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.sum(torch.square(asset.data.applied_torque[:, asset_cfg.joint_ids]), dim=1) + + +def joint_vel_l1(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize joint velocities on the articulation using an L1-kernel.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.sum(torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids]), dim=1) + + +def joint_vel_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize joint velocities on the articulation using L2 squared kernel. + + NOTE: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their joint velocities contribute to the term. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.sum(torch.square(asset.data.joint_vel[:, asset_cfg.joint_ids]), dim=1) + + +def joint_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize joint accelerations on the articulation using L2 squared kernel. + + NOTE: Only the joints configured in :attr:`asset_cfg.joint_ids` will have their joint accelerations contribute to the term. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.sum(torch.square(asset.data.joint_acc[:, asset_cfg.joint_ids]), dim=1) + + +def joint_deviation_l1(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize joint positions that deviate from the default one.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute out of limits constraints + angle = asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids] + return torch.sum(torch.abs(angle), dim=1) + + +def joint_pos_limits(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize joint positions if they cross the soft limits. + + This is computed as a sum of the absolute value of the difference between the joint position and the soft limits. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute out of limits constraints + out_of_limits = -( + asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 0] + ).clip(max=0.0) + out_of_limits += ( + asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 1] + ).clip(min=0.0) + return torch.sum(out_of_limits, dim=1) + + +def joint_vel_limits( + env: ManagerBasedRLEnv, soft_ratio: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Penalize joint velocities if they cross the soft limits. + + This is computed as a sum of the absolute value of the difference between the joint velocity and the soft limits. + + Args: + soft_ratio: The ratio of the soft limits to be used. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute out of limits constraints + out_of_limits = ( + torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids]) + - asset.data.soft_joint_vel_limits[:, asset_cfg.joint_ids] * soft_ratio + ) + # clip to max error = 1 rad/s per joint to avoid huge penalties + out_of_limits = out_of_limits.clip_(min=0.0, max=1.0) + return torch.sum(out_of_limits, dim=1) + + +""" +Action penalties. +""" + + +def applied_torque_limits(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize applied torques if they cross the limits. + + This is computed as a sum of the absolute value of the difference between the applied torques and the limits. + + .. caution:: + Currently, this only works for explicit actuators since we manually compute the applied torques. + For implicit actuators, we currently cannot retrieve the applied torques from the physics engine. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute out of limits constraints + # TODO: We need to fix this to support implicit joints. + out_of_limits = torch.abs( + asset.data.applied_torque[:, asset_cfg.joint_ids] - asset.data.computed_torque[:, asset_cfg.joint_ids] + ) + return torch.sum(out_of_limits, dim=1) + + +def action_rate_l2(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize the rate of change of the actions using L2 squared kernel.""" + return torch.sum(torch.square(env.action_manager.action - env.action_manager.prev_action), dim=1) + + +def action_l2(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize the actions using L2 squared kernel.""" + return torch.sum(torch.square(env.action_manager.action), dim=1) + + +""" +Contact sensor. +""" + + +def undesired_contacts(env: ManagerBasedRLEnv, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize undesired contacts as the number of violations that are above a threshold.""" + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + # check if contact force is above threshold + net_contact_forces = contact_sensor.data.net_forces_w_history + is_contact = torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] > threshold + # sum over contacts for each environment + return torch.sum(is_contact, dim=1) + + +def contact_forces(env: ManagerBasedRLEnv, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize contact forces as the amount of violations of the net contact force.""" + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + net_contact_forces = contact_sensor.data.net_forces_w_history + # compute the violation + violation = torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] - threshold + # compute the penalty + return torch.sum(violation.clip(min=0.0), dim=1) + + +""" +Velocity-tracking rewards. +""" + + +def track_lin_vel_xy_exp( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of linear velocity commands (xy axes) using exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + # compute the error + lin_vel_error = torch.sum( + torch.square(env.command_manager.get_command(command_name)[:, :2] - asset.data.root_lin_vel_b[:, :2]), + dim=1, + ) + return torch.exp(-lin_vel_error / std**2) + + +def track_ang_vel_z_exp( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of angular velocity commands (yaw) using exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + # compute the error + ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_b[:, 2]) + return torch.exp(-ang_vel_error / std**2) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/terminations.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/terminations.py new file mode 100644 index 00000000000..24996aa0f69 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mdp/terminations.py @@ -0,0 +1,163 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensor + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + from isaaclab.managers.command_manager import CommandTerm + +""" +MDP terminations. +""" + + +def time_out(env: ManagerBasedRLEnv) -> torch.Tensor: + """Terminate the episode when the episode length exceeds the maximum episode length.""" + return env.episode_length_buf >= env.max_episode_length + + +def command_resample(env: ManagerBasedRLEnv, command_name: str, num_resamples: int = 1) -> torch.Tensor: + """Terminate the episode based on the total number of times commands have been re-sampled. + + This makes the maximum episode length fluid in nature as it depends on how the commands are + sampled. It is useful in situations where delayed rewards are used :cite:`rudin2022advanced`. + """ + command: CommandTerm = env.command_manager.get_term(command_name) + return torch.logical_and((command.time_left <= env.step_dt), (command.command_counter == num_resamples)) + + +""" +Root terminations. +""" + + +def bad_orientation( + env: ManagerBasedRLEnv, limit_angle: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when the asset's orientation is too far from the desired orientation limits. + + This is computed by checking the angle between the projected gravity vector and the z-axis. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return torch.acos(-asset.data.projected_gravity_b[:, 2]).abs() > limit_angle + + +def root_height_below_minimum( + env: ManagerBasedRLEnv, minimum_height: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when the asset's root height is below the minimum height. + + Note: + This is currently only supported for flat terrains, i.e. the minimum height is in the world frame. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_pos_w[:, 2] < minimum_height + + +""" +Joint terminations. +""" + + +def joint_pos_out_of_limit(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Terminate when the asset's joint positions are outside of the soft joint limits.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute any violations + out_of_upper_limits = torch.any(asset.data.joint_pos > asset.data.soft_joint_pos_limits[..., 1], dim=1) + out_of_lower_limits = torch.any(asset.data.joint_pos < asset.data.soft_joint_pos_limits[..., 0], dim=1) + return torch.logical_or(out_of_upper_limits[:, asset_cfg.joint_ids], out_of_lower_limits[:, asset_cfg.joint_ids]) + + +def joint_pos_out_of_manual_limit( + env: ManagerBasedRLEnv, bounds: tuple[float, float], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when the asset's joint positions are outside of the configured bounds. + + Note: + This function is similar to :func:`joint_pos_out_of_limit` but allows the user to specify the bounds manually. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + if asset_cfg.joint_ids is None: + asset_cfg.joint_ids = slice(None) + # compute any violations + out_of_upper_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] > bounds[1], dim=1) + out_of_lower_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] < bounds[0], dim=1) + return torch.logical_or(out_of_upper_limits, out_of_lower_limits) + + +def joint_vel_out_of_limit(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Terminate when the asset's joint velocities are outside of the soft joint limits.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute any violations + limits = asset.data.soft_joint_vel_limits + return torch.any(torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids]) > limits[:, asset_cfg.joint_ids], dim=1) + + +def joint_vel_out_of_manual_limit( + env: ManagerBasedRLEnv, max_velocity: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when the asset's joint velocities are outside the provided limits.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute any violations + return torch.any(torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids]) > max_velocity, dim=1) + + +def joint_effort_out_of_limit( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when effort applied on the asset's joints are outside of the soft joint limits. + + In the actuators, the applied torque are the efforts applied on the joints. These are computed by clipping + the computed torques to the joint limits. Hence, we check if the computed torques are equal to the applied + torques. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # check if any joint effort is out of limit + out_of_limits = torch.isclose( + asset.data.computed_torque[:, asset_cfg.joint_ids], asset.data.applied_torque[:, asset_cfg.joint_ids] + ) + return torch.any(out_of_limits, dim=1) + + +""" +Contact sensor. +""" + + +def illegal_contact(env: ManagerBasedRLEnv, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Terminate when the contact force on the sensor exceeds the force threshold.""" + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + net_contact_forces = contact_sensor.data.net_forces_w_history + # check if any contact force exceeds the threshold + return torch.any( + torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] > threshold, dim=1 + ) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mimic_env_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mimic_env_cfg.py new file mode 100644 index 00000000000..6c411f17a15 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/mimic_env_cfg.py @@ -0,0 +1,307 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the NVIDIA Source Code License [see LICENSE for details]. + +""" +Base MimicEnvCfg object for Isaac Lab Mimic data generation. +""" +import enum + +from isaaclab.utils import configclass + + +@configclass +class DataGenConfig: + """Configuration settings for data generation processes within the Isaac Lab Mimic environment.""" + + name: str = "demo" + """The name of the data generation process. Defaults to "demo".""" + + generation_guarantee: bool = True + """Whether to retry generation until generation_num_trials successful demos have been generated. + + If True, generation will be retried until generation_num_trials successful demos are created. + If False, generation will stop after generation_num_trails, regardless of success. + """ + + generation_keep_failed: bool = False + """Whether to keep failed generation trials. + + Keeping failed demonstrations is useful for visualizing and debugging low success rates. + """ + + max_num_failures: int = 50 + """Maximum number of failures allowed before stopping generation.""" + + seed: int = 1 + """Seed for randomization to ensure reproducibility.""" + + """The following configuration values can be changed on the command line, and only serve as defaults.""" + + source_dataset_path: str = None + """Path to the source dataset for mimic generation.""" + + generation_path: str = None + """Path where the generated data will be saved.""" + + generation_num_trials: int = 10 + """Number of trials to be generated.""" + + task_name: str = None + """Name of the task being configured.""" + + """The following configurations are advanced and do not usually need to be changed.""" + + generation_select_src_per_subtask: bool = False + """Whether to select source data per subtask. + + Note: + This requires subtasks to be properly temporally constrained, and may require + additional subtasks to allow for time synchronization. + """ + + generation_select_src_per_arm: bool = False + """Whether to select source data per arm.""" + + generation_transform_first_robot_pose: bool = False + """Whether to transform the first robot pose during generation.""" + + generation_interpolate_from_last_target_pose: bool = True + """Whether to interpolate from last target pose.""" + + +@configclass +class SubTaskConfig: + """ + Configuration settings for specifying subtasks used in Mimic environments. + """ + + """Mandatory options that should be defined for every subtask.""" + + object_ref: str = None + """Reference to the object involved in this subtask. + + Set to None if no object is involved (this is rarely the case). + """ + + subtask_term_signal: str = None + """Subtask termination signal name.""" + + """Advanced options for tuning the generation results.""" + + selection_strategy: str = "random" + """Strategy for selecting a subtask segment. + + Can be one of: + * 'random' + * 'nearest_neighbor_object' + * 'nearest_neighbor_robot_distance' + + Note: + For 'nearest_neighbor_object' and 'nearest_neighbor_robot_distance', the subtask needs + to have 'object_ref' set to a value other than 'None'. These strategies typically yield + higher success rates than the default 'random' strategy when object_ref is set. + """ + + selection_strategy_kwargs: dict = {} + """Additional arguments to the selected strategy. See details on each strategy in + source/isaaclab_mimic/isaaclab_mimic/datagen/selection_strategy.py + Arguments will be passed through to the `select_source_demo` method.""" + + first_subtask_start_offset_range: tuple = (0, 0) + """Range for start offset of the first subtask.""" + + subtask_term_offset_range: tuple = (0, 0) + """Range for offsetting subtask termination.""" + + action_noise: float = 0.03 + """Amplitude of action noise applied.""" + + num_interpolation_steps: int = 5 + """Number of steps for interpolation between waypoints.""" + + num_fixed_steps: int = 0 + """Number of fixed steps for the subtask.""" + + apply_noise_during_interpolation: bool = False + """Whether to apply noise during interpolation.""" + + description: str = "" + """Description of the subtask""" + + next_subtask_description: str = "" + """Instructions for the next subtask""" + + +class SubTaskConstraintType(enum.IntEnum): + """Enum for subtask constraint types.""" + + SEQUENTIAL = 0 + COORDINATION = 1 + + _SEQUENTIAL_FORMER = 2 + _SEQUENTIAL_LATTER = 3 + + +class SubTaskConstraintCoordinationScheme(enum.IntEnum): + """Enum for coordination schemes.""" + + REPLAY = 0 + TRANSFORM = 1 + TRANSLATE = 2 + + +@configclass +class SubTaskConstraintConfig: + """ + Configuration settings for specifying subtask constraints used in multi-eef Mimic environments. + """ + + eef_subtask_constraint_tuple: list[tuple[str, int]] = (("", 0), ("", 0)) + """List of associated subtasks tuples in order. + + The first element of the tuple refers to the eef name. + The second element of the tuple refers to the subtask index of the eef. + """ + + constraint_type: SubTaskConstraintType = None + """Type of constraint to apply between subtasks.""" + + sequential_min_time_diff: int = -1 + """Minimum time difference between two sequential subtasks finishing. + + The second subtask will execute until sequential_min_time_diff steps left in its subtask trajectory + and wait until the first (preconditioned) subtask is finished to continue executing the rest. + If set to -1, the second subtask will start only after the first subtask is finished. + """ + + coordination_scheme: SubTaskConstraintCoordinationScheme = SubTaskConstraintCoordinationScheme.REPLAY + """Scheme to use for coordinating subtasks.""" + + coordination_scheme_pos_noise_scale: float = 0.0 + """Scale of position noise to apply during coordination.""" + + coordination_scheme_rot_noise_scale: float = 0.0 + """Scale of rotation noise to apply during coordination.""" + + coordination_synchronize_start: bool = False + """Whether subtasks should start at the same time.""" + + def generate_runtime_subtask_constraints(self): + """ + Populate expanded task constraints dictionary based on the task constraint config. + The task constraint config contains the configurations set by the user. While the + task_constraints_dict contains flags used to implement the constraint logic in this class. + + The task_constraint_configs may include the following types: + - "sequential" + - "coordination" + + For a "sequential" constraint: + - Data from task_constraint_configs is added to task_constraints_dict as "sequential former" task constraint. + - The opposite constraint, of type "sequential latter", is also added to task_constraints_dict. + - Additionally, a ("fulfilled", Bool) key-value pair is added to task_constraints_dict. + - This is used to check if the precondition (i.e., the sequential former task) has been met. + - Until the "fulfilled" flag in "sequential latter" is set by "sequential former", + the "sequential latter" subtask will remain paused. + + For a "coordination" constraint: + - Data from task_constraint_configs is added to task_constraints_dict. + - The opposite constraint, of type "coordination", is also added to task_constraints_dict. + - The number of synchronous steps is set to the minimum of subtask_len and concurrent_subtask_len. + - This ensures both concurrent tasks end at the same time step. + - A "selected_src_demo_ind" and "transform" field are used to ensure the transforms used by both subtasks are the same. + """ + task_constraints_dict = dict() + if self.constraint_type == SubTaskConstraintType.SEQUENTIAL: + constrained_task_spec_key, constrained_subtask_ind = self.eef_subtask_constraint_tuple[1] + assert isinstance(constrained_subtask_ind, int) + pre_condition_task_spec_key, pre_condition_subtask_ind = self.eef_subtask_constraint_tuple[0] + assert isinstance(pre_condition_subtask_ind, int) + assert ( + constrained_task_spec_key, + constrained_subtask_ind, + ) not in task_constraints_dict, "only one constraint per subtask allowed" + task_constraints_dict[(constrained_task_spec_key, constrained_subtask_ind)] = dict( + type=SubTaskConstraintType._SEQUENTIAL_LATTER, + pre_condition_task_spec_key=pre_condition_task_spec_key, + pre_condition_subtask_ind=pre_condition_subtask_ind, + min_time_diff=self.sequential_min_time_diff, + fulfilled=False, + ) + task_constraints_dict[(pre_condition_task_spec_key, pre_condition_subtask_ind)] = dict( + type=SubTaskConstraintType._SEQUENTIAL_FORMER, + constrained_task_spec_key=constrained_task_spec_key, + constrained_subtask_ind=constrained_subtask_ind, + ) + elif self.constraint_type == SubTaskConstraintType.COORDINATION: + constrained_task_spec_key, constrained_subtask_ind = self.eef_subtask_constraint_tuple[0] + assert isinstance(constrained_subtask_ind, int) + concurrent_task_spec_key, concurrent_subtask_ind = self.eef_subtask_constraint_tuple[1] + assert isinstance(concurrent_subtask_ind, int) + if self.coordination_scheme is None: + raise ValueError("Coordination scheme must be specified.") + assert ( + constrained_task_spec_key, + constrained_subtask_ind, + ) not in task_constraints_dict, "only one constraint per subtask allowed" + task_constraints_dict[(constrained_task_spec_key, constrained_subtask_ind)] = dict( + concurrent_task_spec_key=concurrent_task_spec_key, + concurrent_subtask_ind=concurrent_subtask_ind, + type=SubTaskConstraintType.COORDINATION, + fulfilled=False, + finished=False, + selected_src_demo_ind=None, + coordination_scheme=self.coordination_scheme, + coordination_scheme_pos_noise_scale=self.coordination_scheme_pos_noise_scale, + coordination_scheme_rot_noise_scale=self.coordination_scheme_rot_noise_scale, + coordination_synchronize_start=self.coordination_synchronize_start, + synchronous_steps=None, # to be calculated at runtime + ) + task_constraints_dict[(concurrent_task_spec_key, concurrent_subtask_ind)] = dict( + concurrent_task_spec_key=constrained_task_spec_key, + concurrent_subtask_ind=constrained_subtask_ind, + type=SubTaskConstraintType.COORDINATION, + fulfilled=False, + finished=False, + selected_src_demo_ind=None, + coordination_scheme=self.coordination_scheme, + coordination_scheme_pos_noise_scale=self.coordination_scheme_pos_noise_scale, + coordination_scheme_rot_noise_scale=self.coordination_scheme_rot_noise_scale, + coordination_synchronize_start=self.coordination_synchronize_start, + synchronous_steps=None, # to be calculated at runtime + ) + else: + raise ValueError("Constraint type not supported.") + + return task_constraints_dict + + +@configclass +class MimicEnvCfg: + """ + Configuration class for the Mimic environment integration. + + This class consolidates various configuration aspects for the + Isaac Lab Mimic data generation pipeline. + """ + + # Overall configuration for the data generation + datagen_config: DataGenConfig = DataGenConfig() + + # Dictionary of list of subtask configurations for each end-effector. + # Keys are end-effector names. + subtask_configs: dict[str, list[SubTaskConfig]] = {} + + # List of configurations for subtask constraints + task_constraint_configs: list[SubTaskConstraintConfig] = [] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/__init__.py new file mode 100644 index 00000000000..57227a5e37b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module providing UI window implementation for environments. + +The UI elements are used to control the environment and visualize the state of the environment. +This includes functionalities such as tracking a robot in the simulation, +toggling different debug visualization tools, and other user-defined functionalities. +""" + +from .base_env_window import BaseEnvWindow +from .empty_window import EmptyWindow +from .manager_based_rl_env_window import ManagerBasedRLEnvWindow +from .viewport_camera_controller import ViewportCameraController diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/base_env_window.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/base_env_window.py new file mode 100644 index 00000000000..ae77facc79f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/base_env_window.py @@ -0,0 +1,460 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import asyncio +import os +import weakref +from datetime import datetime +from typing import TYPE_CHECKING + +import isaacsim +import omni.kit.app +import omni.kit.commands +import omni.usd +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics + +from isaaclab.ui.widgets import ManagerLiveVisualizer + +if TYPE_CHECKING: + import omni.ui + + from ..manager_based_env import ManagerBasedEnv + + +class BaseEnvWindow: + """Window manager for the basic environment. + + This class creates a window that is used to control the environment. The window + contains controls for rendering, debug visualization, and other environment-specific + UI elements. + + Users can add their own UI elements to the window by using the `with` context manager. + This can be done either be inheriting the class or by using the `env.window` object + directly from the standalone execution script. + + Example for adding a UI element from the standalone execution script: + >>> with env.window.ui_window_elements["main_vstack"]: + >>> ui.Label("My UI element") + + """ + + def __init__(self, env: ManagerBasedEnv, window_name: str = "IsaacLab"): + """Initialize the window. + + Args: + env: The environment object. + window_name: The name of the window. Defaults to "IsaacLab". + """ + # store inputs + self.env = env + # prepare the list of assets that can be followed by the viewport camera + # note that the first two options are "World" and "Env" which are special cases + self._viewer_assets_options = [ + "World", + "Env", + *self.env.scene.rigid_objects.keys(), + *self.env.scene.articulations.keys(), + ] + + # Listeners for environment selection changes + self._ui_listeners: list[ManagerLiveVisualizer] = [] + + print("Creating window for environment.") + # create window for UI + self.ui_window = omni.ui.Window( + window_name, width=400, height=500, visible=True, dock_preference=omni.ui.DockPreference.RIGHT_TOP + ) + # dock next to properties window + asyncio.ensure_future(self._dock_window(window_title=self.ui_window.title)) + + # keep a dictionary of stacks so that child environments can add their own UI elements + # this can be done by using the `with` context manager + self.ui_window_elements = dict() + # create main frame + self.ui_window_elements["main_frame"] = self.ui_window.frame + with self.ui_window_elements["main_frame"]: + # create main stack + self.ui_window_elements["main_vstack"] = omni.ui.VStack(spacing=5, height=0) + with self.ui_window_elements["main_vstack"]: + # create collapsable frame for simulation + self._build_sim_frame() + # create collapsable frame for viewer + self._build_viewer_frame() + # create collapsable frame for debug visualization + self._build_debug_vis_frame() + with self.ui_window_elements["debug_frame"]: + with self.ui_window_elements["debug_vstack"]: + self._visualize_manager(title="Actions", class_name="action_manager") + self._visualize_manager(title="Observations", class_name="observation_manager") + + def __del__(self): + """Destructor for the window.""" + # destroy the window + if self.ui_window is not None: + self.ui_window.visible = False + self.ui_window.destroy() + self.ui_window = None + + """ + Build sub-sections of the UI. + """ + + def _build_sim_frame(self): + """Builds the sim-related controls frame for the UI.""" + # create collapsable frame for controls + self.ui_window_elements["sim_frame"] = omni.ui.CollapsableFrame( + title="Simulation Settings", + width=omni.ui.Fraction(1), + height=0, + collapsed=False, + style=isaacsim.gui.components.ui_utils.get_style(), + horizontal_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ) + with self.ui_window_elements["sim_frame"]: + # create stack for controls + self.ui_window_elements["sim_vstack"] = omni.ui.VStack(spacing=5, height=0) + with self.ui_window_elements["sim_vstack"]: + # create rendering mode dropdown + render_mode_cfg = { + "label": "Rendering Mode", + "type": "dropdown", + "default_val": self.env.sim.render_mode.value, + "items": [member.name for member in self.env.sim.RenderMode if member.value >= 0], + "tooltip": "Select a rendering mode\n" + self.env.sim.RenderMode.__doc__, + "on_clicked_fn": lambda value: self.env.sim.set_render_mode(self.env.sim.RenderMode[value]), + } + self.ui_window_elements["render_dropdown"] = isaacsim.gui.components.ui_utils.dropdown_builder( + **render_mode_cfg + ) + + # create animation recording box + record_animate_cfg = { + "label": "Record Animation", + "type": "state_button", + "a_text": "START", + "b_text": "STOP", + "tooltip": "Record the animation of the scene. Only effective if fabric is disabled.", + "on_clicked_fn": lambda value: self._toggle_recording_animation_fn(value), + } + self.ui_window_elements["record_animation"] = isaacsim.gui.components.ui_utils.state_btn_builder( + **record_animate_cfg + ) + # disable the button if fabric is not enabled + self.ui_window_elements["record_animation"].enabled = not self.env.sim.is_fabric_enabled() + + def _build_viewer_frame(self): + """Build the viewer-related control frame for the UI.""" + # create collapsable frame for viewer + self.ui_window_elements["viewer_frame"] = omni.ui.CollapsableFrame( + title="Viewer Settings", + width=omni.ui.Fraction(1), + height=0, + collapsed=False, + style=isaacsim.gui.components.ui_utils.get_style(), + horizontal_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ) + with self.ui_window_elements["viewer_frame"]: + # create stack for controls + self.ui_window_elements["viewer_vstack"] = omni.ui.VStack(spacing=5, height=0) + with self.ui_window_elements["viewer_vstack"]: + # create a number slider to move to environment origin + # NOTE: slider is 1-indexed, whereas the env index is 0-indexed + viewport_origin_cfg = { + "label": "Environment Index", + "type": "button", + "default_val": self.env.cfg.viewer.env_index + 1, + "min": 1, + "max": self.env.num_envs, + "tooltip": "The environment index to follow. Only effective if follow mode is not 'World'.", + } + self.ui_window_elements["viewer_env_index"] = isaacsim.gui.components.ui_utils.int_builder( + **viewport_origin_cfg + ) + # create a number slider to move to environment origin + self.ui_window_elements["viewer_env_index"].add_value_changed_fn(self._set_viewer_env_index_fn) + + # create a tracker for the camera location + viewer_follow_cfg = { + "label": "Follow Mode", + "type": "dropdown", + "default_val": 0, + "items": [name.replace("_", " ").title() for name in self._viewer_assets_options], + "tooltip": "Select the viewport camera following mode.", + "on_clicked_fn": self._set_viewer_origin_type_fn, + } + self.ui_window_elements["viewer_follow"] = isaacsim.gui.components.ui_utils.dropdown_builder( + **viewer_follow_cfg + ) + + # add viewer default eye and lookat locations + self.ui_window_elements["viewer_eye"] = isaacsim.gui.components.ui_utils.xyz_builder( + label="Camera Eye", + tooltip="Modify the XYZ location of the viewer eye.", + default_val=self.env.cfg.viewer.eye, + step=0.1, + on_value_changed_fn=[self._set_viewer_location_fn] * 3, + ) + self.ui_window_elements["viewer_lookat"] = isaacsim.gui.components.ui_utils.xyz_builder( + label="Camera Target", + tooltip="Modify the XYZ location of the viewer target.", + default_val=self.env.cfg.viewer.lookat, + step=0.1, + on_value_changed_fn=[self._set_viewer_location_fn] * 3, + ) + + def _build_debug_vis_frame(self): + """Builds the debug visualization frame for various scene elements. + + This function inquires the scene for all elements that have a debug visualization + implemented and creates a checkbox to toggle the debug visualization for each element + that has it implemented. If the element does not have a debug visualization implemented, + a label is created instead. + """ + # create collapsable frame for debug visualization + self.ui_window_elements["debug_frame"] = omni.ui.CollapsableFrame( + title="Scene Debug Visualization", + width=omni.ui.Fraction(1), + height=0, + collapsed=False, + style=isaacsim.gui.components.ui_utils.get_style(), + horizontal_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=omni.ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ) + with self.ui_window_elements["debug_frame"]: + # create stack for debug visualization + self.ui_window_elements["debug_vstack"] = omni.ui.VStack(spacing=5, height=0) + with self.ui_window_elements["debug_vstack"]: + elements = [ + self.env.scene.terrain, + *self.env.scene.rigid_objects.values(), + *self.env.scene.articulations.values(), + *self.env.scene.sensors.values(), + ] + names = [ + "terrain", + *self.env.scene.rigid_objects.keys(), + *self.env.scene.articulations.keys(), + *self.env.scene.sensors.keys(), + ] + # create one for the terrain + for elem, name in zip(elements, names): + if elem is not None: + self._create_debug_vis_ui_element(name, elem) + + def _visualize_manager(self, title: str, class_name: str) -> None: + """Checks if the attribute with the name 'class_name' can be visualized. If yes, create vis interface. + + Args: + title: The title of the manager visualization frame. + class_name: The name of the manager to visualize. + """ + + if hasattr(self.env, class_name) and class_name in self.env.manager_visualizers: + manager = self.env.manager_visualizers[class_name] + if hasattr(manager, "has_debug_vis_implementation"): + self._create_debug_vis_ui_element(title, manager) + else: + print( + f"ManagerLiveVisualizer cannot be created for manager: {class_name}, has_debug_vis_implementation" + " does not exist" + ) + else: + print(f"ManagerLiveVisualizer cannot be created for manager: {class_name}, Manager does not exist") + + """ + Custom callbacks for UI elements. + """ + + def _toggle_recording_animation_fn(self, value: bool): + """Toggles the animation recording.""" + if value: + # log directory to save the recording + if not hasattr(self, "animation_log_dir"): + # create a new log directory + log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.animation_log_dir = os.path.join(os.getcwd(), "recordings", log_dir) + # start the recording + _ = omni.kit.commands.execute( + "StartRecording", + target_paths=[("/World", True)], + live_mode=True, + use_frame_range=False, + start_frame=0, + end_frame=0, + use_preroll=False, + preroll_frame=0, + record_to="FILE", + fps=0, + apply_root_anim=False, + increment_name=True, + record_folder=self.animation_log_dir, + take_name="TimeSample", + ) + else: + # stop the recording + _ = omni.kit.commands.execute("StopRecording") + # save the current stage + stage = omni.usd.get_context().get_stage() + source_layer = stage.GetRootLayer() + # output the stage to a file + stage_usd_path = os.path.join(self.animation_log_dir, "Stage.usd") + source_prim_path = "/" + # creates empty anon layer + temp_layer = Sdf.Find(stage_usd_path) + if temp_layer is None: + temp_layer = Sdf.Layer.CreateNew(stage_usd_path) + temp_stage = Usd.Stage.Open(temp_layer) + # update stage data + UsdGeom.SetStageUpAxis(temp_stage, UsdGeom.GetStageUpAxis(stage)) + UsdGeom.SetStageMetersPerUnit(temp_stage, UsdGeom.GetStageMetersPerUnit(stage)) + # copy the prim + Sdf.CreatePrimInLayer(temp_layer, source_prim_path) + Sdf.CopySpec(source_layer, source_prim_path, temp_layer, source_prim_path) + # set the default prim + temp_layer.defaultPrim = Sdf.Path(source_prim_path).name + # remove all physics from the stage + for prim in temp_stage.TraverseAll(): + # skip if the prim is an instance + if prim.IsInstanceable(): + continue + # if prim has articulation then disable it + if prim.HasAPI(UsdPhysics.ArticulationRootAPI): + prim.RemoveAPI(UsdPhysics.ArticulationRootAPI) + prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI) + # if prim has rigid body then disable it + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + prim.RemoveAPI(UsdPhysics.RigidBodyAPI) + prim.RemoveAPI(PhysxSchema.PhysxRigidBodyAPI) + # if prim is a joint type then disable it + if prim.IsA(UsdPhysics.Joint): + prim.GetAttribute("physics:jointEnabled").Set(False) + # resolve all paths relative to layer path + omni.usd.resolve_paths(source_layer.identifier, temp_layer.identifier) + # save the stage + temp_layer.Save() + # print the path to the saved stage + print("Recording completed.") + print(f"\tSaved recorded stage to : {stage_usd_path}") + print(f"\tSaved recorded animation to: {os.path.join(self.animation_log_dir, 'TimeSample_tk001.usd')}") + print("\nTo play the animation, check the instructions in the following link:") + print( + "\thttps://docs.omniverse.nvidia.com/extensions/latest/ext_animation_stage-recorder.html#using-the-captured-timesamples" + ) + print("\n") + # reset the log directory + self.animation_log_dir = None + + def _set_viewer_origin_type_fn(self, value: str): + """Sets the origin of the viewport's camera. This is based on the drop-down menu in the UI.""" + # Extract the viewport camera controller from environment + vcc = self.env.viewport_camera_controller + if vcc is None: + raise ValueError("Viewport camera controller is not initialized! Please check the rendering mode.") + + # Based on origin type, update the camera view + if value == "World": + vcc.update_view_to_world() + elif value == "Env": + vcc.update_view_to_env() + else: + # find which index the asset is + fancy_names = [name.replace("_", " ").title() for name in self._viewer_assets_options] + # store the desired env index + viewer_asset_name = self._viewer_assets_options[fancy_names.index(value)] + # update the camera view + vcc.update_view_to_asset_root(viewer_asset_name) + + def _set_viewer_location_fn(self, model: omni.ui.SimpleFloatModel): + """Sets the viewport camera location based on the UI.""" + # access the viewport camera controller (for brevity) + vcc = self.env.viewport_camera_controller + if vcc is None: + raise ValueError("Viewport camera controller is not initialized! Please check the rendering mode.") + # obtain the camera locations and set them in the viewpoint camera controller + eye = [self.ui_window_elements["viewer_eye"][i].get_value_as_float() for i in range(3)] + lookat = [self.ui_window_elements["viewer_lookat"][i].get_value_as_float() for i in range(3)] + # update the camera view + vcc.update_view_location(eye, lookat) + + def _set_viewer_env_index_fn(self, model: omni.ui.SimpleIntModel): + """Sets the environment index and updates the camera if in 'env' origin mode.""" + # access the viewport camera controller (for brevity) + vcc = self.env.viewport_camera_controller + if vcc is None: + raise ValueError("Viewport camera controller is not initialized! Please check the rendering mode.") + # store the desired env index, UI is 1-indexed + vcc.set_view_env_index(model.as_int - 1) + # notify additional listeners + for listener in self._ui_listeners: + listener.set_env_selection(model.as_int - 1) + + """ + Helper functions - UI building. + """ + + def _create_debug_vis_ui_element(self, name: str, elem: object): + """Create a checkbox for toggling debug visualization for the given element.""" + from omni.kit.window.extensions import SimpleCheckBox + + with omni.ui.HStack(): + # create the UI element + text = ( + "Toggle debug visualization." + if elem.has_debug_vis_implementation + else "Debug visualization not implemented." + ) + omni.ui.Label( + name.replace("_", " ").title(), + width=isaacsim.gui.components.ui_utils.LABEL_WIDTH - 12, + alignment=omni.ui.Alignment.LEFT_CENTER, + tooltip=text, + ) + has_cfg = hasattr(elem, "cfg") and elem.cfg is not None + is_checked = False + if has_cfg: + is_checked = (hasattr(elem.cfg, "debug_vis") and elem.cfg.debug_vis) or ( + hasattr(elem, "debug_vis") and elem.debug_vis + ) + self.ui_window_elements[f"{name}_cb"] = SimpleCheckBox( + model=omni.ui.SimpleBoolModel(), + enabled=elem.has_debug_vis_implementation, + checked=is_checked, + on_checked_fn=lambda value, e=weakref.proxy(elem): e.set_debug_vis(value), + ) + isaacsim.gui.components.ui_utils.add_line_rect_flourish() + + # Create a panel for the debug visualization + if isinstance(elem, ManagerLiveVisualizer): + self.ui_window_elements[f"{name}_panel"] = omni.ui.Frame(width=omni.ui.Fraction(1)) + if not elem.set_vis_frame(self.ui_window_elements[f"{name}_panel"]): + print(f"Frame failed to set for ManagerLiveVisualizer: {name}") + + # Add listener for environment selection changes + if isinstance(elem, ManagerLiveVisualizer): + self._ui_listeners.append(elem) + + async def _dock_window(self, window_title: str): + """Docks the custom UI window to the property window.""" + # wait for the window to be created + for _ in range(5): + if omni.ui.Workspace.get_window(window_title): + break + await self.env.sim.app.next_update_async() + + # dock next to properties window + custom_window = omni.ui.Workspace.get_window(window_title) + property_window = omni.ui.Workspace.get_window("Property") + if custom_window and property_window: + custom_window.dock_in(property_window, omni.ui.DockPosition.SAME, 1.0) + custom_window.focus() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/empty_window.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/empty_window.py new file mode 100644 index 00000000000..052b9132b10 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/empty_window.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import asyncio +from typing import TYPE_CHECKING + +import omni.kit.app + +if TYPE_CHECKING: + import omni.ui + + from ..manager_based_env import ManagerBasedEnv + + +class EmptyWindow: + """ + Creates an empty UI window that can be docked in the Omniverse Kit environment. + + The class initializes a dockable UI window and provides a main frame with a vertical stack. + You can add custom UI elements to this vertical stack. + + Example for adding a UI element from the standalone execution script: + >>> with env.window.ui_window_elements["main_vstack"]: + >>> ui.Label("My UI element") + + """ + + def __init__(self, env: ManagerBasedEnv, window_name: str): + """Initialize the window. + + Args: + env: The environment object. + window_name: The name of the window. + """ + # store environment + self.env = env + + # create window for UI + self.ui_window = omni.ui.Window( + window_name, width=400, height=500, visible=True, dock_preference=omni.ui.DockPreference.RIGHT_TOP + ) + # dock next to properties window + asyncio.ensure_future(self._dock_window(window_title=self.ui_window.title)) + + # keep a dictionary of stacks so that child environments can add their own UI elements + # this can be done by using the `with` context manager + self.ui_window_elements = dict() + # create main frame + self.ui_window_elements["main_frame"] = self.ui_window.frame + with self.ui_window_elements["main_frame"]: + # create main vstack + self.ui_window_elements["main_vstack"] = omni.ui.VStack(spacing=5, height=0) + + def __del__(self): + """Destructor for the window.""" + # destroy the window + if self.ui_window is not None: + self.ui_window.visible = False + self.ui_window.destroy() + self.ui_window = None + + async def _dock_window(self, window_title: str): + """Docks the custom UI window to the property window.""" + # wait for the window to be created + for _ in range(5): + if omni.ui.Workspace.get_window(window_title): + break + await self.env.sim.app.next_update_async() + + # dock next to properties window + custom_window = omni.ui.Workspace.get_window(window_title) + property_window = omni.ui.Workspace.get_window("Property") + if custom_window and property_window: + custom_window.dock_in(property_window, omni.ui.DockPosition.SAME, 1.0) + custom_window.focus() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/manager_based_rl_env_window.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/manager_based_rl_env_window.py new file mode 100644 index 00000000000..fbfdda7122a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/manager_based_rl_env_window.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from .base_env_window import BaseEnvWindow + +if TYPE_CHECKING: + from ..manager_based_rl_env import ManagerBasedRLEnv + + +class ManagerBasedRLEnvWindow(BaseEnvWindow): + """Window manager for the RL environment. + + On top of the basic environment window, this class adds controls for the RL environment. + This includes visualization of the command manager. + """ + + def __init__(self, env: ManagerBasedRLEnv, window_name: str = "IsaacLab"): + """Initialize the window. + + Args: + env: The environment object. + window_name: The name of the window. Defaults to "IsaacLab". + """ + # initialize base window + super().__init__(env, window_name) + + # add custom UI elements + with self.ui_window_elements["main_vstack"]: + with self.ui_window_elements["debug_frame"]: + with self.ui_window_elements["debug_vstack"]: + self._visualize_manager(title="Commands", class_name="command_manager") + self._visualize_manager(title="Rewards", class_name="reward_manager") + self._visualize_manager(title="Curriculum", class_name="curriculum_manager") + self._visualize_manager(title="Termination", class_name="termination_manager") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/viewport_camera_controller.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/viewport_camera_controller.py new file mode 100644 index 00000000000..4a0e0209565 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/ui/viewport_camera_controller.py @@ -0,0 +1,237 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import copy +import numpy as np +import torch +import weakref +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.kit.app +import omni.timeline + +from isaaclab.assets.articulation.articulation import Articulation + +if TYPE_CHECKING: + from isaaclab.envs import DirectRLEnv, ManagerBasedEnv, ViewerCfg + + +class ViewportCameraController: + """This class handles controlling the camera associated with a viewport in the simulator. + + It can be used to set the viewpoint camera to track different origin types: + + - **world**: the center of the world (static) + - **env**: the center of an environment (static) + - **asset_root**: the root of an asset in the scene (e.g. tracking a robot moving in the scene) + + On creation, the camera is set to track the origin type specified in the configuration. + + For the :attr:`asset_root` origin type, the camera is updated at each rendering step to track the asset's + root position. For this, it registers a callback to the post update event stream from the simulation app. + """ + + def __init__(self, env: ManagerBasedEnv | DirectRLEnv, cfg: ViewerCfg): + """Initialize the ViewportCameraController. + + Args: + env: The environment. + cfg: The configuration for the viewport camera controller. + + Raises: + ValueError: If origin type is configured to be "env" but :attr:`cfg.env_index` is out of bounds. + ValueError: If origin type is configured to be "asset_root" but :attr:`cfg.asset_name` is unset. + + """ + # store inputs + self._env = env + self._cfg = copy.deepcopy(cfg) + # cast viewer eye and look-at to numpy arrays + self.default_cam_eye = np.array(self._cfg.eye) + self.default_cam_lookat = np.array(self._cfg.lookat) + + # set the camera origins + if self.cfg.origin_type == "env": + # check that the env_index is within bounds + self.set_view_env_index(self.cfg.env_index) + # set the camera origin to the center of the environment + self.update_view_to_env() + elif self.cfg.origin_type == "asset_root" or self.cfg.origin_type == "asset_body": + # note: we do not yet update camera for tracking an asset origin, as the asset may not yet be + # in the scene when this is called. Instead, we subscribe to the post update event to update the camera + # at each rendering step. + if self.cfg.asset_name is None: + raise ValueError(f"No asset name provided for viewer with origin type: '{self.cfg.origin_type}'.") + if self.cfg.origin_type == "asset_body": + if self.cfg.body_name is None: + raise ValueError(f"No body name provided for viewer with origin type: '{self.cfg.origin_type}'.") + else: + # set the camera origin to the center of the world + self.update_view_to_world() + + # subscribe to post update event so that camera view can be updated at each rendering step + app_interface = omni.kit.app.get_app_interface() + app_event_stream = app_interface.get_post_update_event_stream() + self._viewport_camera_update_handle = app_event_stream.create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._update_tracking_callback(event) + ) + + def __del__(self): + """Unsubscribe from the callback.""" + # use hasattr to handle case where __init__ has not completed before __del__ is called + if hasattr(self, "_viewport_camera_update_handle") and self._viewport_camera_update_handle is not None: + self._viewport_camera_update_handle.unsubscribe() + self._viewport_camera_update_handle = None + + """ + Properties + """ + + @property + def cfg(self) -> ViewerCfg: + """The configuration for the viewer.""" + return self._cfg + + """ + Public Functions + """ + + def set_view_env_index(self, env_index: int): + """Sets the environment index for the camera view. + + Args: + env_index: The index of the environment to set the camera view to. + + Raises: + ValueError: If the environment index is out of bounds. It should be between 0 and num_envs - 1. + """ + # check that the env_index is within bounds + if env_index < 0 or env_index >= self._env.num_envs: + raise ValueError( + f"Out of range value for attribute 'env_index': {env_index}." + f" Expected a value between 0 and {self._env.num_envs - 1} for the current environment." + ) + # update the environment index + self.cfg.env_index = env_index + # update the camera view if the origin is set to env type (since, the camera view is static) + # note: for assets, the camera view is updated at each rendering step + if self.cfg.origin_type == "env": + self.update_view_to_env() + + def update_view_to_world(self): + """Updates the viewer's origin to the origin of the world which is (0, 0, 0).""" + # set origin type to world + self.cfg.origin_type = "world" + # update the camera origins + self.viewer_origin = torch.zeros(3) + # update the camera view + self.update_view_location() + + def update_view_to_env(self): + """Updates the viewer's origin to the origin of the selected environment.""" + # set origin type to world + self.cfg.origin_type = "env" + # update the camera origins + self.viewer_origin = self._env.scene.env_origins[self.cfg.env_index] + # update the camera view + self.update_view_location() + + def update_view_to_asset_root(self, asset_name: str): + """Updates the viewer's origin based upon the root of an asset in the scene. + + Args: + asset_name: The name of the asset in the scene. The name should match the name of the + asset in the scene. + + Raises: + ValueError: If the asset is not in the scene. + """ + # check if the asset is in the scene + if self.cfg.asset_name != asset_name: + asset_entities = [*self._env.scene.rigid_objects.keys(), *self._env.scene.articulations.keys()] + if asset_name not in asset_entities: + raise ValueError(f"Asset '{asset_name}' is not in the scene. Available entities: {asset_entities}.") + # update the asset name + self.cfg.asset_name = asset_name + # set origin type to asset_root + self.cfg.origin_type = "asset_root" + # update the camera origins + self.viewer_origin = self._env.scene[self.cfg.asset_name].data.root_pos_w[self.cfg.env_index] + # update the camera view + self.update_view_location() + + def update_view_to_asset_body(self, asset_name: str, body_name: str): + """Updates the viewer's origin based upon the body of an asset in the scene. + + Args: + asset_name: The name of the asset in the scene. The name should match the name of the + asset in the scene. + body_name: The name of the body in the asset. + + Raises: + ValueError: If the asset is not in the scene or the body is not valid. + """ + # check if the asset is in the scene + if self.cfg.asset_name != asset_name: + asset_entities = [*self._env.scene.rigid_objects.keys(), *self._env.scene.articulations.keys()] + if asset_name not in asset_entities: + raise ValueError(f"Asset '{asset_name}' is not in the scene. Available entities: {asset_entities}.") + # check if the body is in the asset + asset: Articulation = self._env.scene[asset_name] + if body_name not in asset.body_names: + raise ValueError( + f"'{body_name}' is not a body of Asset '{asset_name}'. Available bodies: {asset.body_names}." + ) + # get the body index + body_id, _ = asset.find_bodies(body_name) + # update the asset name + self.cfg.asset_name = asset_name + # set origin type to asset_body + self.cfg.origin_type = "asset_body" + # update the camera origins + self.viewer_origin = self._env.scene[self.cfg.asset_name].data.body_pos_w[self.cfg.env_index, body_id].view(3) + # update the camera view + self.update_view_location() + + def update_view_location(self, eye: Sequence[float] | None = None, lookat: Sequence[float] | None = None): + """Updates the camera view pose based on the current viewer origin and the eye and lookat positions. + + Args: + eye: The eye position of the camera. If None, the current eye position is used. + lookat: The lookat position of the camera. If None, the current lookat position is used. + """ + # store the camera view pose for later use + if eye is not None: + self.default_cam_eye = np.asarray(eye) + if lookat is not None: + self.default_cam_lookat = np.asarray(lookat) + # set the camera locations + viewer_origin = self.viewer_origin.detach().cpu().numpy() + cam_eye = viewer_origin + self.default_cam_eye + cam_target = viewer_origin + self.default_cam_lookat + + # set the camera view + self._env.sim.set_camera_view(eye=cam_eye, target=cam_target) + + """ + Private Functions + """ + + def _update_tracking_callback(self, event): + """Updates the camera view at each rendering step.""" + # update the camera view if the origin is set to asset_root + # in other cases, the camera view is static and does not need to be updated continuously + if self.cfg.origin_type == "asset_root" and self.cfg.asset_name is not None: + self.update_view_to_asset_root(self.cfg.asset_name) + if self.cfg.origin_type == "asset_body" and self.cfg.asset_name is not None and self.cfg.body_name is not None: + self.update_view_to_asset_body(self.cfg.asset_name, self.cfg.body_name) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/__init__.py new file mode 100644 index 00000000000..61c21397ab7 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for environment utils.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/marl.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/marl.py new file mode 100644 index 00000000000..a818e18f67c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/marl.py @@ -0,0 +1,279 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym +import math +import numpy as np +import torch +from typing import Any + +from ..common import ActionType, AgentID, EnvStepReturn, ObsType, StateType, VecEnvObs, VecEnvStepReturn +from ..direct_marl_env import DirectMARLEnv +from ..direct_rl_env import DirectRLEnv + + +def multi_agent_to_single_agent(env: DirectMARLEnv, state_as_observation: bool = False) -> DirectRLEnv: + """Convert the multi-agent environment instance to a single-agent environment instance. + + The converted environment will be an instance of the single-agent environment interface class (:class:`DirectRLEnv`). + As part of the conversion process, the following operations are carried out: + + * The observations of all the agents in the original multi-agent environment are concatenated to compose + the single-agent observation. If the use of the environment state is defined as the observation, + it is returned as is. + * The terminations and time-outs of all the agents in the original multi-agent environment are multiplied + (``AND`` operation) to compose the corresponding single-agent values. + * The rewards of all the agents in the original multi-agent environment are summed to compose the + single-agent reward. + * The action taken by the single-agent is split to compose the actions of each agent in the original + multi-agent environment before stepping it. + + Args: + env: The environment to convert to. + state_as_observation: Weather to use the multi-agent environment state as single-agent observation. + + Returns: + Single-agent environment instance. + + Raises: + AssertionError: If the environment state cannot be used as observation since it was explicitly defined + as unconstructed (:attr:`DirectMARLEnvCfg.state_space`). + """ + + class Env(DirectRLEnv): + def __init__(self, env: DirectMARLEnv) -> None: + self.env: DirectMARLEnv = env.unwrapped + + # check if it is possible to use the multi-agent environment state as single-agent observation + self._state_as_observation = state_as_observation + if self._state_as_observation: + assert self.env.cfg.state_space != 0, ( + "The environment state cannot be used as observation since it was explicitly defined as" + " unconstructed" + ) + + # create single-agent properties to expose in the converted environment + self.cfg = self.env.cfg + self.sim = self.env.sim + self.scene = self.env.scene + self.render_mode = self.env.render_mode + + self.single_observation_space = gym.spaces.Dict() + if self._state_as_observation: + self.single_observation_space["policy"] = self.env.state_space + else: + self.single_observation_space["policy"] = gym.spaces.flatten_space( + gym.spaces.Tuple([self.env.observation_spaces[agent] for agent in self.env.possible_agents]) + ) + self.single_action_space = gym.spaces.flatten_space( + gym.spaces.Tuple([self.env.action_spaces[agent] for agent in self.env.possible_agents]) + ) + + # batch the spaces for vectorized environments + self.observation_space = gym.vector.utils.batch_space( + self.single_observation_space["policy"], self.num_envs + ) + self.action_space = gym.vector.utils.batch_space(self.single_action_space, self.num_envs) + + def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[VecEnvObs, dict]: + obs, extras = self.env.reset(seed, options) + + # use environment state as observation + if self._state_as_observation: + obs = {"policy": self.env.state()} + # concatenate agents' observations + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + else: + obs = { + "policy": torch.cat( + [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 + ) + } + + return obs, extras + + def step(self, action: torch.Tensor) -> VecEnvStepReturn: + # split single-agent actions to build the multi-agent ones + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + index = 0 + _actions = {} + for agent in self.env.possible_agents: + delta = gym.spaces.flatdim(self.env.action_spaces[agent]) + _actions[agent] = action[:, index : index + delta] + index += delta + + # step the environment + obs, rewards, terminated, time_outs, extras = self.env.step(_actions) + + # use environment state as observation + if self._state_as_observation: + obs = {"policy": self.env.state()} + # concatenate agents' observations + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + else: + obs = { + "policy": torch.cat( + [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 + ) + } + + # process environment outputs to return single-agent data + rewards = sum(rewards.values()) + terminated = math.prod(terminated.values()).to(dtype=torch.bool) + time_outs = math.prod(time_outs.values()).to(dtype=torch.bool) + + return obs, rewards, terminated, time_outs, extras + + def render(self, recompute: bool = False) -> np.ndarray | None: + return self.env.render(recompute) + + def close(self) -> None: + self.env.close() + + return Env(env) + + +def multi_agent_with_one_agent(env: DirectMARLEnv, state_as_observation: bool = False) -> DirectMARLEnv: + """Convert the multi-agent environment instance to a multi-agent environment instance with only one agent. + + The converted environment will be an instance of the multi-agent environment interface class + (:class:`DirectMARLEnv`) but with only one agent available (with ID: ``"single-agent"``). + As part of the conversion process, the following operations are carried out: + + * The observations of all the agents in the original multi-agent environment are concatenated to compose + the agent observation. If the use of the environment state is defined as the observation, it is returned as is. + * The terminations and time-outs of all the agents in the original multi-agent environment are multiplied + (``AND`` operation) to compose the corresponding agent values. + * The rewards of all the agents in the original multi-agent environment are summed to compose the agent reward. + * The action taken by the agent is split to compose the actions of each agent in the original + multi-agent environment before stepping it. + + Args: + env: The environment to convert to. + state_as_observation: Weather to use the multi-agent environment state as agent observation. + + Returns: + Multi-agent environment instance with only one agent. + + Raises: + AssertionError: If the environment state cannot be used as observation since it was explicitly defined + as unconstructed (:attr:`DirectMARLEnvCfg.state_space`). + """ + + class Env(DirectMARLEnv): + def __init__(self, env: DirectMARLEnv) -> None: + self.env: DirectMARLEnv = env.unwrapped + + # check if it is possible to use the multi-agent environment state as agent observation + self._state_as_observation = state_as_observation + if self._state_as_observation: + assert self.env.cfg.state_space != 0, ( + "The environment state cannot be used as observation since it was explicitly defined as" + " unconstructed" + ) + + # create agent properties to expose in the converted environment + self._agent_id = "single-agent" + self._exported_agents = [self._agent_id] + self._exported_possible_agents = [self._agent_id] + if self._state_as_observation: + self._exported_observation_spaces = {self._agent_id: self.env.state_space} + else: + self._exported_observation_spaces = { + self._agent_id: gym.spaces.flatten_space( + gym.spaces.Tuple([self.env.observation_spaces[agent] for agent in self.env.possible_agents]) + ) + } + self._exported_action_spaces = { + self._agent_id: gym.spaces.flatten_space( + gym.spaces.Tuple([self.env.action_spaces[agent] for agent in self.env.possible_agents]) + ) + } + + def __getattr__(self, key: str) -> Any: + return getattr(self.env, key) + + @property + def agents(self) -> list[AgentID]: + return self._exported_agents + + @property + def possible_agents(self) -> list[AgentID]: + return self._exported_possible_agents + + @property + def observation_spaces(self) -> dict[AgentID, gym.Space]: + return self._exported_observation_spaces + + @property + def action_spaces(self) -> dict[AgentID, gym.Space]: + return self._exported_action_spaces + + def reset( + self, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[AgentID, ObsType], dict[AgentID, dict]]: + obs, extras = self.env.reset(seed, options) + + # use environment state as observation + if self._state_as_observation: + obs = {self._agent_id: self.env.state()} + # concatenate agents' observations + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + else: + obs = { + self._agent_id: torch.cat( + [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 + ) + } + + return obs, extras + + def step(self, actions: dict[AgentID, ActionType]) -> EnvStepReturn: + # split agent actions to build the multi-agent ones + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + index = 0 + _actions = {} + for agent in self.env.possible_agents: + delta = gym.spaces.flatdim(self.env.action_spaces[agent]) + _actions[agent] = actions[self._agent_id][:, index : index + delta] + index += delta + + # step the environment + obs, rewards, terminated, time_outs, extras = self.env.step(_actions) + + # use environment state as observation + if self._state_as_observation: + obs = {self._agent_id: self.env.state()} + # concatenate agents' observations + # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces + else: + obs = { + self._agent_id: torch.cat( + [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 + ) + } + + # process environment outputs to return agent data + rewards = {self._agent_id: sum(rewards.values())} + terminated = {self._agent_id: math.prod(terminated.values()).to(dtype=torch.bool)} + time_outs = {self._agent_id: math.prod(time_outs.values()).to(dtype=torch.bool)} + + return obs, rewards, terminated, time_outs, extras + + def state(self) -> StateType | None: + return self.env.state() + + def render(self, recompute: bool = False) -> np.ndarray | None: + self.env.render(recompute) + + def close(self) -> None: + self.env.close() + + return Env(env) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/spaces.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/spaces.py new file mode 100644 index 00000000000..1be364b842d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/envs/utils/spaces.py @@ -0,0 +1,226 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym +import json +import numpy as np +import torch +from typing import Any + +from ..common import SpaceType + + +def spec_to_gym_space(spec: SpaceType) -> gym.spaces.Space: + """Generate an appropriate Gymnasium space according to the given space specification. + + Args: + spec: Space specification. + + Returns: + Gymnasium space. + + Raises: + ValueError: If the given space specification is not valid/supported. + """ + if isinstance(spec, gym.spaces.Space): + return spec + # fundamental spaces + # Box + elif isinstance(spec, int): + return gym.spaces.Box(low=-np.inf, high=np.inf, shape=(spec,)) + elif isinstance(spec, list) and all(isinstance(x, int) for x in spec): + return gym.spaces.Box(low=-np.inf, high=np.inf, shape=spec) + # Discrete + elif isinstance(spec, set) and len(spec) == 1: + return gym.spaces.Discrete(n=next(iter(spec))) + # MultiDiscrete + elif isinstance(spec, list) and all(isinstance(x, set) and len(x) == 1 for x in spec): + return gym.spaces.MultiDiscrete(nvec=[next(iter(x)) for x in spec]) + # composite spaces + # Tuple + elif isinstance(spec, tuple): + return gym.spaces.Tuple([spec_to_gym_space(x) for x in spec]) + # Dict + elif isinstance(spec, dict): + return gym.spaces.Dict({k: spec_to_gym_space(v) for k, v in spec.items()}) + raise ValueError(f"Unsupported space specification: {spec}") + + +def sample_space(space: gym.spaces.Space, device: str, batch_size: int = -1, fill_value: float | None = None) -> Any: + """Sample a Gymnasium space where the data container are PyTorch tensors. + + Args: + space: Gymnasium space. + device: The device where the tensor should be created. + batch_size: Batch size. If the specified value is greater than zero, a batched space will be created and sampled from it. + fill_value: The value to fill the created tensors with. If None (default value), tensors will keep their random values. + + Returns: + Tensorized sampled space. + """ + + def tensorize(s, x): + if isinstance(s, gym.spaces.Box): + tensor = torch.tensor(x, device=device, dtype=torch.float32).reshape(batch_size, *s.shape) + if fill_value is not None: + tensor.fill_(fill_value) + return tensor + elif isinstance(s, gym.spaces.Discrete): + if isinstance(x, np.ndarray): + tensor = torch.tensor(x, device=device, dtype=torch.int64).reshape(batch_size, 1) + if fill_value is not None: + tensor.fill_(int(fill_value)) + return tensor + elif isinstance(x, np.number) or type(x) in [int, float]: + tensor = torch.tensor([x], device=device, dtype=torch.int64).reshape(batch_size, 1) + if fill_value is not None: + tensor.fill_(int(fill_value)) + return tensor + elif isinstance(s, gym.spaces.MultiDiscrete): + if isinstance(x, np.ndarray): + tensor = torch.tensor(x, device=device, dtype=torch.int64).reshape(batch_size, *s.shape) + if fill_value is not None: + tensor.fill_(int(fill_value)) + return tensor + elif isinstance(s, gym.spaces.Dict): + return {k: tensorize(_s, x[k]) for k, _s in s.items()} + elif isinstance(s, gym.spaces.Tuple): + return tuple([tensorize(_s, v) for _s, v in zip(s, x)]) + + sample = (gym.vector.utils.batch_space(space, batch_size) if batch_size > 0 else space).sample() + return tensorize(space, sample) + + +def serialize_space(space: SpaceType) -> str: + """Serialize a space specification as JSON. + + Args: + space: Space specification. + + Returns: + Serialized JSON representation. + """ + # Gymnasium spaces + if isinstance(space, gym.spaces.Discrete): + return json.dumps({"type": "gymnasium", "space": "Discrete", "n": int(space.n)}) + elif isinstance(space, gym.spaces.Box): + return json.dumps({ + "type": "gymnasium", + "space": "Box", + "low": space.low.tolist(), + "high": space.high.tolist(), + "shape": space.shape, + }) + elif isinstance(space, gym.spaces.MultiDiscrete): + return json.dumps({"type": "gymnasium", "space": "MultiDiscrete", "nvec": space.nvec.tolist()}) + elif isinstance(space, gym.spaces.Tuple): + return json.dumps({"type": "gymnasium", "space": "Tuple", "spaces": tuple(map(serialize_space, space.spaces))}) + elif isinstance(space, gym.spaces.Dict): + return json.dumps( + {"type": "gymnasium", "space": "Dict", "spaces": {k: serialize_space(v) for k, v in space.spaces.items()}} + ) + # Python data types + # Box + elif isinstance(space, int) or (isinstance(space, list) and all(isinstance(x, int) for x in space)): + return json.dumps({"type": "python", "space": "Box", "value": space}) + # Discrete + elif isinstance(space, set) and len(space) == 1: + return json.dumps({"type": "python", "space": "Discrete", "value": next(iter(space))}) + # MultiDiscrete + elif isinstance(space, list) and all(isinstance(x, set) and len(x) == 1 for x in space): + return json.dumps({"type": "python", "space": "MultiDiscrete", "value": [next(iter(x)) for x in space]}) + # composite spaces + # Tuple + elif isinstance(space, tuple): + return json.dumps({"type": "python", "space": "Tuple", "value": [serialize_space(x) for x in space]}) + # Dict + elif isinstance(space, dict): + return json.dumps( + {"type": "python", "space": "Dict", "value": {k: serialize_space(v) for k, v in space.items()}} + ) + raise ValueError(f"Unsupported space ({space})") + + +def deserialize_space(string: str) -> gym.spaces.Space: + """Deserialize a space specification encoded as JSON. + + Args: + string: Serialized JSON representation. + + Returns: + Space specification. + """ + obj = json.loads(string) + # Gymnasium spaces + if obj["type"] == "gymnasium": + if obj["space"] == "Discrete": + return gym.spaces.Discrete(n=obj["n"]) + elif obj["space"] == "Box": + return gym.spaces.Box(low=np.array(obj["low"]), high=np.array(obj["high"]), shape=obj["shape"]) + elif obj["space"] == "MultiDiscrete": + return gym.spaces.MultiDiscrete(nvec=np.array(obj["nvec"])) + elif obj["space"] == "Tuple": + return gym.spaces.Tuple(spaces=tuple(map(deserialize_space, obj["spaces"]))) + elif obj["space"] == "Dict": + return gym.spaces.Dict(spaces={k: deserialize_space(v) for k, v in obj["spaces"].items()}) + else: + raise ValueError(f"Unsupported space ({obj['spaces']})") + # Python data types + elif obj["type"] == "python": + if obj["space"] == "Discrete": + return {obj["value"]} + elif obj["space"] == "Box": + return obj["value"] + elif obj["space"] == "MultiDiscrete": + return [{x} for x in obj["value"]] + elif obj["space"] == "Tuple": + return tuple(map(deserialize_space, obj["value"])) + elif obj["space"] == "Dict": + return {k: deserialize_space(v) for k, v in obj["value"].items()} + else: + raise ValueError(f"Unsupported space ({obj['spaces']})") + else: + raise ValueError(f"Unsupported type ({obj['type']})") + + +def replace_env_cfg_spaces_with_strings(env_cfg: object) -> object: + """Replace spaces objects with their serialized JSON representations in an environment config. + + Args: + env_cfg: Environment config instance. + + Returns: + Environment config instance with spaces replaced if any. + """ + for attr in ["observation_space", "action_space", "state_space"]: + if hasattr(env_cfg, attr): + setattr(env_cfg, attr, serialize_space(getattr(env_cfg, attr))) + for attr in ["observation_spaces", "action_spaces"]: + if hasattr(env_cfg, attr): + setattr(env_cfg, attr, {k: serialize_space(v) for k, v in getattr(env_cfg, attr).items()}) + return env_cfg + + +def replace_strings_with_env_cfg_spaces(env_cfg: object) -> object: + """Replace spaces objects with their serialized JSON representations in an environment config. + + Args: + env_cfg: Environment config instance. + + Returns: + Environment config instance with spaces replaced if any. + """ + for attr in ["observation_space", "action_space", "state_space"]: + if hasattr(env_cfg, attr): + setattr(env_cfg, attr, deserialize_space(getattr(env_cfg, attr))) + for attr in ["observation_spaces", "action_spaces"]: + if hasattr(env_cfg, attr): + setattr(env_cfg, attr, {k: deserialize_space(v) for k, v in getattr(env_cfg, attr).items()}) + return env_cfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/__init__.py new file mode 100644 index 00000000000..c0bb23a896e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/__init__.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for environment managers. + +The managers are used to handle various aspects of the environment such as randomization events, curriculum, +and observations. Each manager implements a specific functionality for the environment. The managers are +designed to be modular and can be easily extended to support new functionality. +""" + +from .action_manager import ActionManager, ActionTerm +from .command_manager import CommandManager, CommandTerm +from .curriculum_manager import CurriculumManager +from .event_manager import EventManager +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import ( + ActionTermCfg, + CommandTermCfg, + CurriculumTermCfg, + EventTermCfg, + ManagerTermBaseCfg, + ObservationGroupCfg, + ObservationTermCfg, + RecorderTermCfg, + RewardTermCfg, + TerminationTermCfg, +) +from .observation_manager import ObservationManager +from .recorder_manager import DatasetExportMode, RecorderManager, RecorderManagerBaseCfg, RecorderTerm +from .reward_manager import RewardManager +from .scene_entity_cfg import SceneEntityCfg +from .termination_manager import TerminationManager diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/action_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/action_manager.py new file mode 100644 index 00000000000..f9b5996afe2 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/action_manager.py @@ -0,0 +1,406 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Action manager for processing actions sent to the environment.""" + +from __future__ import annotations + +import inspect +import torch +import weakref +from abc import abstractmethod +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +import omni.kit.app + +from isaaclab.assets import AssetBase + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import ActionTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class ActionTerm(ManagerTermBase): + """Base class for action terms. + + The action term is responsible for processing the raw actions sent to the environment + and applying them to the asset managed by the term. The action term is comprised of two + operations: + + * Processing of actions: This operation is performed once per **environment step** and + is responsible for pre-processing the raw actions sent to the environment. + * Applying actions: This operation is performed once per **simulation step** and is + responsible for applying the processed actions to the asset managed by the term. + """ + + def __init__(self, cfg: ActionTermCfg, env: ManagerBasedEnv): + """Initialize the action term. + + Args: + cfg: The configuration object. + env: The environment instance. + """ + # call the base class constructor + super().__init__(cfg, env) + # parse config to obtain asset to which the term is applied + self._asset: AssetBase = self._env.scene[self.cfg.asset_name] + + # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) + self._debug_vis_handle = None + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + + def __del__(self): + """Unsubscribe from the callbacks.""" + if self._debug_vis_handle: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + """ + Properties. + """ + + @property + @abstractmethod + def action_dim(self) -> int: + """Dimension of the action term.""" + raise NotImplementedError + + @property + @abstractmethod + def raw_actions(self) -> torch.Tensor: + """The input/raw actions sent to the term.""" + raise NotImplementedError + + @property + @abstractmethod + def processed_actions(self) -> torch.Tensor: + """The actions computed by the term after applying any processing.""" + raise NotImplementedError + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the action term has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_debug_vis_impl) + return "NotImplementedError" not in source_code + + """ + Operations. + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Sets whether to visualize the action term data. + Args: + debug_vis: Whether to visualize the action term data. + Returns: + Whether the debug visualization was successfully set. False if the action term does + not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + @abstractmethod + def process_actions(self, actions: torch.Tensor): + """Processes the actions sent to the environment. + + Note: + This function is called once per environment step by the manager. + + Args: + actions: The actions to process. + """ + raise NotImplementedError + + @abstractmethod + def apply_actions(self): + """Applies the actions to the asset managed by the term. + + Note: + This is called at every simulation step by the manager. + """ + raise NotImplementedError + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + def _debug_vis_callback(self, event): + """Callback for debug visualization. + This function calls the visualization objects and sets the data to visualize into them. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + +class ActionManager(ManagerBase): + """Manager for processing and applying actions for a given world. + + The action manager handles the interpretation and application of user-defined + actions on a given world. It is comprised of different action terms that decide + the dimension of the expected actions. + + The action manager performs operations at two stages: + + * processing of actions: It splits the input actions to each term and performs any + pre-processing needed. This should be called once at every environment step. + * apply actions: This operation typically sets the processed actions into the assets in the + scene (such as robots). It should be called before every simulation step. + """ + + def __init__(self, cfg: object, env: ManagerBasedEnv): + """Initialize the action manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, ActionTermCfg]``). + env: The environment instance. + + Raises: + ValueError: If the configuration is None. + """ + # check if config is None + if cfg is None: + raise ValueError("Action manager configuration is None. Please provide a valid configuration.") + + # call the base class constructor (this prepares the terms) + super().__init__(cfg, env) + # create buffers to store actions + self._action = torch.zeros((self.num_envs, self.total_action_dim), device=self.device) + self._prev_action = torch.zeros_like(self._action) + + # check if any term has debug visualization implemented + self.cfg.debug_vis = False + for term in self._terms.values(): + self.cfg.debug_vis |= term.cfg.debug_vis + + def __str__(self) -> str: + """Returns: A string representation for action manager.""" + msg = f" contains {len(self._term_names)} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = f"Active Action Terms (shape: {self.total_action_dim})" + table.field_names = ["Index", "Name", "Dimension"] + # set alignment of table columns + table.align["Name"] = "l" + table.align["Dimension"] = "r" + # add info on each term + for index, (name, term) in enumerate(self._terms.items()): + table.add_row([index, name, term.action_dim]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def total_action_dim(self) -> int: + """Total dimension of actions.""" + return sum(self.action_term_dim) + + @property + def active_terms(self) -> list[str]: + """Name of active action terms.""" + return self._term_names + + @property + def action_term_dim(self) -> list[int]: + """Shape of each action term.""" + return [term.action_dim for term in self._terms.values()] + + @property + def action(self) -> torch.Tensor: + """The actions sent to the environment. Shape is (num_envs, total_action_dim).""" + return self._action + + @property + def prev_action(self) -> torch.Tensor: + """The previous actions sent to the environment. Shape is (num_envs, total_action_dim).""" + return self._prev_action + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the command terms have debug visualization implemented.""" + # check if function raises NotImplementedError + has_debug_vis = False + for term in self._terms.values(): + has_debug_vis |= term.has_debug_vis_implementation + return has_debug_vis + + """ + Operations. + """ + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + terms = [] + idx = 0 + for name, term in self._terms.items(): + term_actions = self._action[env_idx, idx : idx + term.action_dim].cpu() + terms.append((name, term_actions.tolist())) + idx += term.action_dim + return terms + + def set_debug_vis(self, debug_vis: bool): + """Sets whether to visualize the action data. + Args: + debug_vis: Whether to visualize the action data. + Returns: + Whether the debug visualization was successfully set. False if the action + does not support debug visualization. + """ + for term in self._terms.values(): + term.set_debug_vis(debug_vis) + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Resets the action history. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + + Returns: + An empty dictionary. + """ + # resolve environment ids + if env_ids is None: + env_ids = slice(None) + # reset the action history + self._prev_action[env_ids] = 0.0 + self._action[env_ids] = 0.0 + # reset all action terms + for term in self._terms.values(): + term.reset(env_ids=env_ids) + # nothing to log here + return {} + + def process_action(self, action: torch.Tensor): + """Processes the actions sent to the environment. + + Note: + This function should be called once per environment step. + + Args: + action: The actions to process. + """ + # check if action dimension is valid + if self.total_action_dim != action.shape[1]: + raise ValueError(f"Invalid action shape, expected: {self.total_action_dim}, received: {action.shape[1]}.") + # store the input actions + self._prev_action[:] = self._action + self._action[:] = action.to(self.device) + + # split the actions and apply to each tensor + idx = 0 + for term in self._terms.values(): + term_actions = action[:, idx : idx + term.action_dim] + term.process_actions(term_actions) + idx += term.action_dim + + def apply_action(self) -> None: + """Applies the actions to the environment/simulation. + + Note: + This should be called at every simulation step. + """ + for term in self._terms.values(): + term.apply_actions() + + def get_term(self, name: str) -> ActionTerm: + """Returns the action term with the specified name. + + Args: + name: The name of the action term. + + Returns: + The action term with the specified name. + """ + return self._terms[name] + + def serialize(self) -> dict: + """Serialize the action manager configuration. + + Returns: + A dictionary of serialized action term configurations. + """ + return {term_name: term.serialize() for term_name, term in self._terms.items()} + + """ + Helper functions. + """ + + def _prepare_terms(self): + # create buffers to parse and store terms + self._term_names: list[str] = list() + self._terms: dict[str, ActionTerm] = dict() + + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # parse action terms from the config + for term_name, term_cfg in cfg_items: + # check if term config is None + if term_cfg is None: + continue + # check valid type + if not isinstance(term_cfg, ActionTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type ActionTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # create the action term + term = term_cfg.class_type(term_cfg, self._env) + # sanity check if term is valid type + if not isinstance(term, ActionTerm): + raise TypeError(f"Returned object for the term '{term_name}' is not of type ActionType.") + # add term name and parameters + self._term_names.append(term_name) + self._terms[term_name] = term diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/command_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/command_manager.py new file mode 100644 index 00000000000..2ba1b5f5ab5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/command_manager.py @@ -0,0 +1,428 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Command manager for generating and updating commands.""" + +from __future__ import annotations + +import inspect +import torch +import weakref +from abc import abstractmethod +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +import omni.kit.app + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import CommandTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class CommandTerm(ManagerTermBase): + """The base class for implementing a command term. + + A command term is used to generate commands for goal-conditioned tasks. For example, + in the case of a goal-conditioned navigation task, the command term can be used to + generate a target position for the robot to navigate to. + + It implements a resampling mechanism that allows the command to be resampled at a fixed + frequency. The resampling frequency can be specified in the configuration object. + Additionally, it is possible to assign a visualization function to the command term + that can be used to visualize the command in the simulator. + """ + + def __init__(self, cfg: CommandTermCfg, env: ManagerBasedRLEnv): + """Initialize the command generator class. + + Args: + cfg: The configuration parameters for the command generator. + env: The environment object. + """ + super().__init__(cfg, env) + + # create buffers to store the command + # -- metrics that can be used for logging + self.metrics = dict() + # -- time left before resampling + self.time_left = torch.zeros(self.num_envs, device=self.device) + # -- counter for the number of times the command has been resampled within the current episode + self.command_counter = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + + # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) + self._debug_vis_handle = None + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + + def __del__(self): + """Unsubscribe from the callbacks.""" + if self._debug_vis_handle: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + """ + Properties + """ + + @property + @abstractmethod + def command(self) -> torch.Tensor: + """The command tensor. Shape is (num_envs, command_dim).""" + raise NotImplementedError + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the command generator has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_debug_vis_impl) + return "NotImplementedError" not in source_code + + """ + Operations. + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Sets whether to visualize the command data. + + Args: + debug_vis: Whether to visualize the command data. + + Returns: + Whether the debug visualization was successfully set. False if the command + generator does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + """Reset the command generator and log metrics. + + This function resets the command counter and resamples the command. It should be called + at the beginning of each episode. + + Args: + env_ids: The list of environment IDs to reset. Defaults to None. + + Returns: + A dictionary containing the information to log under the "{name}" key. + """ + # resolve the environment IDs + if env_ids is None: + env_ids = slice(None) + + # add logging metrics + extras = {} + for metric_name, metric_value in self.metrics.items(): + # compute the mean metric value + extras[metric_name] = torch.mean(metric_value[env_ids]).item() + # reset the metric value + metric_value[env_ids] = 0.0 + + # set the command counter to zero + self.command_counter[env_ids] = 0 + # resample the command + self._resample(env_ids) + + return extras + + def compute(self, dt: float): + """Compute the command. + + Args: + dt: The time step passed since the last call to compute. + """ + # update the metrics based on current state + self._update_metrics() + # reduce the time left before resampling + self.time_left -= dt + # resample the command if necessary + resample_env_ids = (self.time_left <= 0.0).nonzero().flatten() + if len(resample_env_ids) > 0: + self._resample(resample_env_ids) + # update the command + self._update_command() + + """ + Helper functions. + """ + + def _resample(self, env_ids: Sequence[int]): + """Resample the command. + + This function resamples the command and time for which the command is applied for the + specified environment indices. + + Args: + env_ids: The list of environment IDs to resample. + """ + if len(env_ids) != 0: + # resample the time left before resampling + self.time_left[env_ids] = self.time_left[env_ids].uniform_(*self.cfg.resampling_time_range) + # resample the command + self._resample_command(env_ids) + # increment the command counter + self.command_counter[env_ids] += 1 + + """ + Implementation specific functions. + """ + + @abstractmethod + def _update_metrics(self): + """Update the metrics based on the current state.""" + raise NotImplementedError + + @abstractmethod + def _resample_command(self, env_ids: Sequence[int]): + """Resample the command for the specified environments.""" + raise NotImplementedError + + @abstractmethod + def _update_command(self): + """Update the command based on the current state.""" + raise NotImplementedError + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + def _debug_vis_callback(self, event): + """Callback for debug visualization. + + This function calls the visualization objects and sets the data to visualize into them. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + +class CommandManager(ManagerBase): + """Manager for generating commands. + + The command manager is used to generate commands for an agent to execute. It makes it convenient to switch + between different command generation strategies within the same environment. For instance, in an environment + consisting of a quadrupedal robot, the command to it could be a velocity command or position command. + By keeping the command generation logic separate from the environment, it is easy to switch between different + command generation strategies. + + The command terms are implemented as classes that inherit from the :class:`CommandTerm` class. + Each command generator term should also have a corresponding configuration class that inherits from the + :class:`CommandTermCfg` class. + """ + + _env: ManagerBasedRLEnv + """The environment instance.""" + + def __init__(self, cfg: object, env: ManagerBasedRLEnv): + """Initialize the command manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, CommandTermCfg]``). + env: The environment instance. + """ + # create buffers to parse and store terms + self._terms: dict[str, CommandTerm] = dict() + + # call the base class constructor (this prepares the terms) + super().__init__(cfg, env) + # store the commands + self._commands = dict() + if self.cfg: + self.cfg.debug_vis = False + for term in self._terms.values(): + self.cfg.debug_vis |= term.cfg.debug_vis + + def __str__(self) -> str: + """Returns: A string representation for the command manager.""" + msg = f" contains {len(self._terms.values())} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = "Active Command Terms" + table.field_names = ["Index", "Name", "Type"] + # set alignment of table columns + table.align["Name"] = "l" + # add info on each term + for index, (name, term) in enumerate(self._terms.items()): + table.add_row([index, name, term.__class__.__name__]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def active_terms(self) -> list[str]: + """Name of active command terms.""" + return list(self._terms.keys()) + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the command terms have debug visualization implemented.""" + # check if function raises NotImplementedError + has_debug_vis = False + for term in self._terms.values(): + has_debug_vis |= term.has_debug_vis_implementation + return has_debug_vis + + """ + Operations. + """ + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + + terms = [] + idx = 0 + for name, term in self._terms.items(): + terms.append((name, term.command[env_idx].cpu().tolist())) + idx += term.command.shape[1] + return terms + + def set_debug_vis(self, debug_vis: bool): + """Sets whether to visualize the command data. + + Args: + debug_vis: Whether to visualize the command data. + + Returns: + Whether the debug visualization was successfully set. False if the command + generator does not support debug visualization. + """ + for term in self._terms.values(): + term.set_debug_vis(debug_vis) + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Reset the command terms and log their metrics. + + This function resets the command counter and resamples the command for each term. It should be called + at the beginning of each episode. + + Args: + env_ids: The list of environment IDs to reset. Defaults to None. + + Returns: + A dictionary containing the information to log under the "Metrics/{term_name}/{metric_name}" key. + """ + # resolve environment ids + if env_ids is None: + env_ids = slice(None) + # store information + extras = {} + for name, term in self._terms.items(): + # reset the command term + metrics = term.reset(env_ids=env_ids) + # compute the mean metric value + for metric_name, metric_value in metrics.items(): + extras[f"Metrics/{name}/{metric_name}"] = metric_value + # return logged information + return extras + + def compute(self, dt: float): + """Updates the commands. + + This function calls each command term managed by the class. + + Args: + dt: The time-step interval of the environment. + + """ + # iterate over all the command terms + for term in self._terms.values(): + # compute term's value + term.compute(dt) + + def get_command(self, name: str) -> torch.Tensor: + """Returns the command for the specified command term. + + Args: + name: The name of the command term. + + Returns: + The command tensor of the specified command term. + """ + return self._terms[name].command + + def get_term(self, name: str) -> CommandTerm: + """Returns the command term with the specified name. + + Args: + name: The name of the command term. + + Returns: + The command term with the specified name. + """ + return self._terms[name] + + """ + Helper functions. + """ + + def _prepare_terms(self): + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # check for valid config type + if not isinstance(term_cfg, CommandTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type CommandTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # create the action term + term = term_cfg.class_type(term_cfg, self._env) + # sanity check if term is valid type + if not isinstance(term, CommandTerm): + raise TypeError(f"Returned object for the term '{term_name}' is not of type CommandType.") + # add class to dict + self._terms[term_name] = term diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/curriculum_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/curriculum_manager.py new file mode 100644 index 00000000000..b285f50eb38 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/curriculum_manager.py @@ -0,0 +1,208 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Curriculum manager for updating environment quantities subject to a training curriculum.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import CurriculumTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class CurriculumManager(ManagerBase): + """Manager to implement and execute specific curricula. + + The curriculum manager updates various quantities of the environment subject to a training curriculum by + calling a list of terms. These help stabilize learning by progressively making the learning tasks harder + as the agent improves. + + The curriculum terms are parsed from a config class containing the manager's settings and each term's + parameters. Each curriculum term should instantiate the :class:`CurriculumTermCfg` class. + """ + + _env: ManagerBasedRLEnv + """The environment instance.""" + + def __init__(self, cfg: object, env: ManagerBasedRLEnv): + """Initialize the manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, CurriculumTermCfg]``) + env: An environment object. + + Raises: + TypeError: If curriculum term is not of type :class:`CurriculumTermCfg`. + ValueError: If curriculum term configuration does not satisfy its function signature. + """ + # create buffers to parse and store terms + self._term_names: list[str] = list() + self._term_cfgs: list[CurriculumTermCfg] = list() + self._class_term_cfgs: list[CurriculumTermCfg] = list() + + # call the base class constructor (this will parse the terms config) + super().__init__(cfg, env) + + # prepare logging + self._curriculum_state = dict() + for term_name in self._term_names: + self._curriculum_state[term_name] = None + + def __str__(self) -> str: + """Returns: A string representation for curriculum manager.""" + msg = f" contains {len(self._term_names)} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = "Active Curriculum Terms" + table.field_names = ["Index", "Name"] + # set alignment of table columns + table.align["Name"] = "l" + # add info on each term + for index, name in enumerate(self._term_names): + table.add_row([index, name]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def active_terms(self) -> list[str]: + """Name of active curriculum terms.""" + return self._term_names + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + """Returns the current state of individual curriculum terms. + + Note: + This function does not use the environment indices :attr:`env_ids` + and logs the state of all the terms. The argument is only present + to maintain consistency with other classes. + + Returns: + Dictionary of curriculum terms and their states. + """ + extras = {} + for term_name, term_state in self._curriculum_state.items(): + if term_state is not None: + # deal with dict + if isinstance(term_state, dict): + # each key is a separate state to log + for key, value in term_state.items(): + if isinstance(value, torch.Tensor): + value = value.item() + extras[f"Curriculum/{term_name}/{key}"] = value + else: + # log directly if not a dict + if isinstance(term_state, torch.Tensor): + term_state = term_state.item() + extras[f"Curriculum/{term_name}"] = term_state + # reset all the curriculum terms + for term_cfg in self._class_term_cfgs: + term_cfg.func.reset(env_ids=env_ids) + # return logged information + return extras + + def compute(self, env_ids: Sequence[int] | None = None): + """Update the curriculum terms. + + This function calls each curriculum term managed by the class. + + Args: + env_ids: The list of environment IDs to update. + If None, all the environments are updated. Defaults to None. + """ + # resolve environment indices + if env_ids is None: + env_ids = slice(None) + # iterate over all the curriculum terms + for name, term_cfg in zip(self._term_names, self._term_cfgs): + state = term_cfg.func(self._env, env_ids, **term_cfg.params) + self._curriculum_state[name] = state + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + + terms = [] + + for term_name, term_state in self._curriculum_state.items(): + if term_state is not None: + # deal with dict + data = [] + + if isinstance(term_state, dict): + # each key is a separate state to log + for key, value in term_state.items(): + if isinstance(value, torch.Tensor): + value = value.item() + terms[term_name].append(value) + else: + # log directly if not a dict + if isinstance(term_state, torch.Tensor): + term_state = term_state.item() + data.append(term_state) + terms.append((term_name, data)) + + return terms + + """ + Helper functions. + """ + + def _prepare_terms(self): + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # check if the term is a valid term config + if not isinstance(term_cfg, CurriculumTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type CurriculumTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # resolve common parameters + self._resolve_common_term_cfg(term_name, term_cfg, min_argc=2) + # add name and config to list + self._term_names.append(term_name) + self._term_cfgs.append(term_cfg) + # check if the term is a class + if isinstance(term_cfg.func, ManagerTermBase): + self._class_term_cfgs.append(term_cfg) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/event_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/event_manager.py new file mode 100644 index 00000000000..2aee4544ff6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/event_manager.py @@ -0,0 +1,426 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Event manager for orchestrating operations based on different simulation events.""" + +from __future__ import annotations + +import inspect +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +import omni.log + +from .manager_base import ManagerBase +from .manager_term_cfg import EventTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class EventManager(ManagerBase): + """Manager for orchestrating operations based on different simulation events. + + The event manager applies operations to the environment based on different simulation events. For example, + changing the masses of objects or their friction coefficients during initialization/ reset, or applying random + pushes to the robot at a fixed interval of steps. The user can specify several modes of events to fine-tune the + behavior based on when to apply the event. + + The event terms are parsed from a config class containing the manager's settings and each term's + parameters. Each event term should instantiate the :class:`EventTermCfg` class. + + Event terms can be grouped by their mode. The mode is a user-defined string that specifies when + the event term should be applied. This provides the user complete control over when event + terms should be applied. + + For a typical training process, you may want to apply events in the following modes: + + - "prestartup": Event is applied once at the beginning of the training before the simulation starts. + This is used to randomize USD-level properties of the simulation stage. + - "startup": Event is applied once at the beginning of the training once simulation is started. + - "reset": Event is applied at every reset. + - "interval": Event is applied at pre-specified intervals of time. + + However, you can also define your own modes and use them in the training process as you see fit. + For this you will need to add the triggering of that mode in the environment implementation as well. + + .. note:: + + The triggering of operations corresponding to the mode ``"interval"`` are the only mode that are + directly handled by the manager itself. The other modes are handled by the environment implementation. + + """ + + _env: ManagerBasedEnv + """The environment instance.""" + + def __init__(self, cfg: object, env: ManagerBasedEnv): + """Initialize the event manager. + + Args: + cfg: A configuration object or dictionary (``dict[str, EventTermCfg]``). + env: An environment object. + """ + # create buffers to parse and store terms + self._mode_term_names: dict[str, list[str]] = dict() + self._mode_term_cfgs: dict[str, list[EventTermCfg]] = dict() + self._mode_class_term_cfgs: dict[str, list[EventTermCfg]] = dict() + + # call the base class (this will parse the terms config) + super().__init__(cfg, env) + + def __str__(self) -> str: + """Returns: A string representation for event manager.""" + msg = f" contains {len(self._mode_term_names)} active terms.\n" + + # add info on each mode + for mode in self._mode_term_names: + # create table for term information + table = PrettyTable() + table.title = f"Active Event Terms in Mode: '{mode}'" + # add table headers based on mode + if mode == "interval": + table.field_names = ["Index", "Name", "Interval time range (s)"] + table.align["Name"] = "l" + for index, (name, cfg) in enumerate(zip(self._mode_term_names[mode], self._mode_term_cfgs[mode])): + table.add_row([index, name, cfg.interval_range_s]) + else: + table.field_names = ["Index", "Name"] + table.align["Name"] = "l" + for index, name in enumerate(self._mode_term_names[mode]): + table.add_row([index, name]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def active_terms(self) -> dict[str, list[str]]: + """Name of active event terms. + + The keys are the modes of event and the values are the names of the event terms. + """ + return self._mode_term_names + + @property + def available_modes(self) -> list[str]: + """Modes of events.""" + return list(self._mode_term_names.keys()) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + # call all terms that are classes + for mode_cfg in self._mode_class_term_cfgs.values(): + for term_cfg in mode_cfg: + term_cfg.func.reset(env_ids=env_ids) + + # resolve number of environments + if env_ids is None: + num_envs = self._env.num_envs + else: + num_envs = len(env_ids) + # if we are doing interval based events then we need to reset the time left + # when the episode starts. otherwise the counter will start from the last time + # for that environment + if "interval" in self._mode_term_cfgs: + for index, term_cfg in enumerate(self._mode_class_term_cfgs["interval"]): + # sample a new interval and set that as time left + # note: global time events are based on simulation time and not episode time + # so we do not reset them + if not term_cfg.is_global_time: + lower, upper = term_cfg.interval_range_s + sampled_interval = torch.rand(num_envs, device=self.device) * (upper - lower) + lower + self._interval_term_time_left[index][env_ids] = sampled_interval + + # nothing to log here + return {} + + def apply( + self, + mode: str, + env_ids: Sequence[int] | None = None, + dt: float | None = None, + global_env_step_count: int | None = None, + ): + """Calls each event term in the specified mode. + + This function iterates over all the event terms in the specified mode and calls the function + corresponding to the term. The function is called with the environment instance and the environment + indices to apply the event to. + + For the "interval" mode, the function is called when the time interval has passed. This requires + specifying the time step of the environment. + + For the "reset" mode, the function is called when the mode is "reset" and the total number of environment + steps that have happened since the last trigger of the function is equal to its configured parameter for + the number of environment steps between resets. + + Args: + mode: The mode of event. + env_ids: The indices of the environments to apply the event to. + Defaults to None, in which case the event is applied to all environments when applicable. + dt: The time step of the environment. This is only used for the "interval" mode. + Defaults to None to simplify the call for other modes. + global_env_step_count: The total number of environment steps that have happened. This is only used + for the "reset" mode. Defaults to None to simplify the call for other modes. + + Raises: + ValueError: If the mode is ``"interval"`` and the time step is not provided. + ValueError: If the mode is ``"interval"`` and the environment indices are provided. This is an undefined + behavior as the environment indices are computed based on the time left for each environment. + ValueError: If the mode is ``"reset"`` and the total number of environment steps that have happened + is not provided. + """ + # check if mode is valid + if mode not in self._mode_term_names: + omni.log.warn(f"Event mode '{mode}' is not defined. Skipping event.") + return + + # check if mode is interval and dt is not provided + if mode == "interval" and dt is None: + raise ValueError(f"Event mode '{mode}' requires the time-step of the environment.") + if mode == "interval" and env_ids is not None: + raise ValueError( + f"Event mode '{mode}' does not require environment indices. This is an undefined behavior" + " as the environment indices are computed based on the time left for each environment." + ) + # check if mode is reset and env step count is not provided + if mode == "reset" and global_env_step_count is None: + raise ValueError(f"Event mode '{mode}' requires the total number of environment steps to be provided.") + + # iterate over all the event terms + for index, term_cfg in enumerate(self._mode_term_cfgs[mode]): + if mode == "interval": + # extract time left for this term + time_left = self._interval_term_time_left[index] + # update the time left for each environment + time_left -= dt + + # check if the interval has passed and sample a new interval + # note: we compare with a small value to handle floating point errors + if term_cfg.is_global_time: + if time_left < 1e-6: + lower, upper = term_cfg.interval_range_s + sampled_interval = torch.rand(1) * (upper - lower) + lower + self._interval_term_time_left[index][:] = sampled_interval + + # call the event term (with None for env_ids) + term_cfg.func(self._env, None, **term_cfg.params) + else: + valid_env_ids = (time_left < 1e-6).nonzero().flatten() + if len(valid_env_ids) > 0: + lower, upper = term_cfg.interval_range_s + sampled_time = torch.rand(len(valid_env_ids), device=self.device) * (upper - lower) + lower + self._interval_term_time_left[index][valid_env_ids] = sampled_time + + # call the event term + term_cfg.func(self._env, valid_env_ids, **term_cfg.params) + elif mode == "reset": + # obtain the minimum step count between resets + min_step_count = term_cfg.min_step_count_between_reset + # resolve the environment indices + if env_ids is None: + env_ids = slice(None) + + # We bypass the trigger mechanism if min_step_count is zero, i.e. apply term on every reset call. + # This should avoid the overhead of checking the trigger condition. + if min_step_count == 0: + self._reset_term_last_triggered_step_id[index][env_ids] = global_env_step_count + self._reset_term_last_triggered_once[index][env_ids] = True + + # call the event term with the environment indices + term_cfg.func(self._env, env_ids, **term_cfg.params) + else: + # extract last reset step for this term + last_triggered_step = self._reset_term_last_triggered_step_id[index][env_ids] + triggered_at_least_once = self._reset_term_last_triggered_once[index][env_ids] + # compute the steps since last reset + steps_since_triggered = global_env_step_count - last_triggered_step + + # check if the term can be applied after the minimum step count between triggers has passed + valid_trigger = steps_since_triggered >= min_step_count + # check if the term has not been triggered yet (in that case, we trigger it at least once) + # this is usually only needed at the start of the environment + valid_trigger |= (last_triggered_step == 0) & ~triggered_at_least_once + + # select the valid environment indices based on the trigger + if env_ids == slice(None): + valid_env_ids = valid_trigger.nonzero().flatten() + else: + valid_env_ids = env_ids[valid_trigger] + + # reset the last reset step for each environment to the current env step count + if len(valid_env_ids) > 0: + self._reset_term_last_triggered_once[index][valid_env_ids] = True + self._reset_term_last_triggered_step_id[index][valid_env_ids] = global_env_step_count + + # call the event term + term_cfg.func(self._env, valid_env_ids, **term_cfg.params) + else: + # call the event term + term_cfg.func(self._env, env_ids, **term_cfg.params) + + """ + Operations - Term settings. + """ + + def set_term_cfg(self, term_name: str, cfg: EventTermCfg): + """Sets the configuration of the specified term into the manager. + + The method finds the term by name by searching through all the modes. + It then updates the configuration of the term with the first matching name. + + Args: + term_name: The name of the event term. + cfg: The configuration for the event term. + + Raises: + ValueError: If the term name is not found. + """ + term_found = False + for mode, terms in self._mode_term_names.items(): + if term_name in terms: + self._mode_term_cfgs[mode][terms.index(term_name)] = cfg + term_found = True + break + if not term_found: + raise ValueError(f"Event term '{term_name}' not found.") + + def get_term_cfg(self, term_name: str) -> EventTermCfg: + """Gets the configuration for the specified term. + + The method finds the term by name by searching through all the modes. + It then returns the configuration of the term with the first matching name. + + Args: + term_name: The name of the event term. + + Returns: + The configuration of the event term. + + Raises: + ValueError: If the term name is not found. + """ + for mode, terms in self._mode_term_names.items(): + if term_name in terms: + return self._mode_term_cfgs[mode][terms.index(term_name)] + raise ValueError(f"Event term '{term_name}' not found.") + + """ + Helper functions. + """ + + def _prepare_terms(self): + # buffer to store the time left for "interval" mode + # if interval is global, then it is a single value, otherwise it is per environment + self._interval_term_time_left: list[torch.Tensor] = list() + # buffer to store the step count when the term was last triggered for each environment for "reset" mode + self._reset_term_last_triggered_step_id: list[torch.Tensor] = list() + self._reset_term_last_triggered_once: list[torch.Tensor] = list() + + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # check for valid config type + if not isinstance(term_cfg, EventTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type EventTermCfg." + f" Received: '{type(term_cfg)}'." + ) + + if term_cfg.mode != "reset" and term_cfg.min_step_count_between_reset != 0: + omni.log.warn( + f"Event term '{term_name}' has 'min_step_count_between_reset' set to a non-zero value" + " but the mode is not 'reset'. Ignoring the 'min_step_count_between_reset' value." + ) + + # resolve common parameters + self._resolve_common_term_cfg(term_name, term_cfg, min_argc=2) + + # check if mode is pre-startup and scene replication is enabled + if term_cfg.mode == "prestartup" and self._env.scene.cfg.replicate_physics: + raise RuntimeError( + "Scene replication is enabled, which may affect USD-level randomization." + " When assets are replicated, their properties are shared across instances," + " potentially leading to unintended behavior." + " For stable USD-level randomization, please disable scene replication" + " by setting 'replicate_physics' to False in 'InteractiveSceneCfg'." + ) + + # for event terms with mode "prestartup", we assume a callable class term + # can be initialized before the simulation starts. + # this is done to ensure that the USD-level randomization is possible before the simulation starts. + if inspect.isclass(term_cfg.func) and term_cfg.mode == "prestartup": + omni.log.info(f"Initializing term '{term_name}' with class '{term_cfg.func.__name__}'.") + term_cfg.func = term_cfg.func(cfg=term_cfg, env=self._env) + + # check if mode is a new mode + if term_cfg.mode not in self._mode_term_names: + # add new mode + self._mode_term_names[term_cfg.mode] = list() + self._mode_term_cfgs[term_cfg.mode] = list() + self._mode_class_term_cfgs[term_cfg.mode] = list() + # add term name and parameters + self._mode_term_names[term_cfg.mode].append(term_name) + self._mode_term_cfgs[term_cfg.mode].append(term_cfg) + + # check if the term is a class + if inspect.isclass(term_cfg.func): + self._mode_class_term_cfgs[term_cfg.mode].append(term_cfg) + + # resolve the mode of the events + # -- interval mode + if term_cfg.mode == "interval": + if term_cfg.interval_range_s is None: + raise ValueError( + f"Event term '{term_name}' has mode 'interval' but 'interval_range_s' is not specified." + ) + + # sample the time left for global + if term_cfg.is_global_time: + lower, upper = term_cfg.interval_range_s + time_left = torch.rand(1) * (upper - lower) + lower + self._interval_term_time_left.append(time_left) + else: + # sample the time left for each environment + lower, upper = term_cfg.interval_range_s + time_left = torch.rand(self.num_envs, device=self.device) * (upper - lower) + lower + self._interval_term_time_left.append(time_left) + # -- reset mode + elif term_cfg.mode == "reset": + if term_cfg.min_step_count_between_reset < 0: + raise ValueError( + f"Event term '{term_name}' has mode 'reset' but 'min_step_count_between_reset' is" + f" negative: {term_cfg.min_step_count_between_reset}. Please provide a non-negative value." + ) + + # initialize the current step count for each environment to zero + step_count = torch.zeros(self.num_envs, device=self.device, dtype=torch.int32) + self._reset_term_last_triggered_step_id.append(step_count) + # initialize the trigger flag for each environment to zero + no_trigger = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self._reset_term_last_triggered_once.append(no_trigger) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_base.py new file mode 100644 index 00000000000..2295706750e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_base.py @@ -0,0 +1,419 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import copy +import inspect +import weakref +from abc import ABC, abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import omni.log +import omni.timeline + +import isaaclab.utils.string as string_utils +from isaaclab.utils import class_to_dict, string_to_callable + +from .manager_term_cfg import ManagerTermBaseCfg +from .scene_entity_cfg import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class ManagerTermBase(ABC): + """Base class for manager terms. + + Manager term implementations can be functions or classes. If the term is a class, it should + inherit from this base class and implement the required methods. + + Each manager is implemented as a class that inherits from the :class:`ManagerBase` class. Each manager + class should also have a corresponding configuration class that defines the configuration terms for the + manager. Each term should the :class:`ManagerTermBaseCfg` class or its subclass. + + Example pseudo-code for creating a manager: + + .. code-block:: python + + from isaaclab.utils import configclass + from isaaclab.utils.mdp import ManagerBase, ManagerTermBaseCfg + + @configclass + class MyManagerCfg: + + my_term_1: ManagerTermBaseCfg = ManagerTermBaseCfg(...) + my_term_2: ManagerTermBaseCfg = ManagerTermBaseCfg(...) + my_term_3: ManagerTermBaseCfg = ManagerTermBaseCfg(...) + + # define manager instance + my_manager = ManagerBase(cfg=ManagerCfg(), env=env) + + """ + + def __init__(self, cfg: ManagerTermBaseCfg, env: ManagerBasedEnv): + """Initialize the manager term. + + Args: + cfg: The configuration object. + env: The environment instance. + """ + # store the inputs + self.cfg = cfg + self._env = env + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """Number of environments.""" + return self._env.num_envs + + @property + def device(self) -> str: + """Device on which to perform computations.""" + return self._env.device + + @property + def __name__(self) -> str: + """Return the name of the class or subclass.""" + return self.__class__.__name__ + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Resets the manager term. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + """ + pass + + def serialize(self) -> dict: + """General serialization call. Includes the configuration dict.""" + return {"cfg": class_to_dict(self.cfg)} + + def __call__(self, *args) -> Any: + """Returns the value of the term required by the manager. + + In case of a class implementation, this function is called by the manager + to get the value of the term. The arguments passed to this function are + the ones specified in the term configuration (see :attr:`ManagerTermBaseCfg.params`). + + .. attention:: + To be consistent with memory-less implementation of terms with functions, it is + recommended to ensure that the returned mutable quantities are cloned before + returning them. For instance, if the term returns a tensor, it is recommended + to ensure that the returned tensor is a clone of the original tensor. This prevents + the manager from storing references to the tensors and altering the original tensors. + + Args: + *args: Variable length argument list. + + Returns: + The value of the term. + """ + raise NotImplementedError("The method '__call__' should be implemented by the subclass.") + + +class ManagerBase(ABC): + """Base class for all managers.""" + + def __init__(self, cfg: object, env: ManagerBasedEnv): + """Initialize the manager. + + This function is responsible for parsing the configuration object and creating the terms. + + If the simulation is not playing, the scene entities are not resolved immediately. + Instead, the resolution is deferred until the simulation starts. This is done to ensure + that the scene entities are resolved even if the manager is created after the simulation + has already started. + + Args: + cfg: The configuration object. If None, the manager is initialized without any terms. + env: The environment instance. + """ + # store the inputs + self.cfg = copy.deepcopy(cfg) + self._env = env + + # flag for whether the scene entities have been resolved + # if sim is playing, we resolve the scene entities directly while preparing the terms + self._is_scene_entities_resolved = self._env.sim.is_playing() + + # if the simulation is not playing, we use callbacks to trigger the resolution of the scene + # entities configuration. this is needed for cases where the manager is created after the + # simulation, but before the simulation is playing. + # FIXME: Once Isaac Sim supports storing this information as USD schema, we can remove this + # callback and resolve the scene entities directly inside `_prepare_terms`. + if not self._env.sim.is_playing(): + # note: Use weakref on all callbacks to ensure that this object can be deleted when its destructor + # is called + # The order is set to 20 to allow asset/sensor initialization to complete before the scene entities + # are resolved. Those have the order 10. + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._resolve_terms_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj=weakref.proxy(self): obj._resolve_terms_callback(event), + order=20, + ) + else: + self._resolve_terms_handle = None + + # parse config to create terms information + if self.cfg: + self._prepare_terms() + + def __del__(self): + """Delete the manager.""" + if self._resolve_terms_handle: + self._resolve_terms_handle.unsubscribe() + self._resolve_terms_handle = None + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """Number of environments.""" + return self._env.num_envs + + @property + def device(self) -> str: + """Device on which to perform computations.""" + return self._env.device + + @property + @abstractmethod + def active_terms(self) -> list[str] | dict[str, list[str]]: + """Name of active terms.""" + raise NotImplementedError + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + """Resets the manager and returns logging information for the current time-step. + + Args: + env_ids: The environment ids for which to log data. + Defaults None, which logs data for all environments. + + Returns: + Dictionary containing the logging information. + """ + return {} + + def find_terms(self, name_keys: str | Sequence[str]) -> list[str]: + """Find terms in the manager based on the names. + + This function searches the manager for terms based on the names. The names can be + specified as regular expressions or a list of regular expressions. The search is + performed on the active terms in the manager. + + Please check the :meth:`~isaaclab.utils.string_utils.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the term names. + + Returns: + A list of term names that match the input keys. + """ + # resolve search keys + if isinstance(self.active_terms, dict): + list_of_strings = [] + for names in self.active_terms.values(): + list_of_strings.extend(names) + else: + list_of_strings = self.active_terms + + # return the matching names + return string_utils.resolve_matching_names(name_keys, list_of_strings)[1] + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Returns: + The active terms. + """ + raise NotImplementedError + + """ + Implementation specific. + """ + + @abstractmethod + def _prepare_terms(self): + """Prepare terms information from the configuration object.""" + raise NotImplementedError + + """ + Internal callbacks. + """ + + def _resolve_terms_callback(self, event): + """Resolve configurations of terms once the simulation starts. + + Please check the :meth:`_process_term_cfg_at_play` method for more information. + """ + # check if scene entities have been resolved + if self._is_scene_entities_resolved: + return + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # process attributes at runtime + # these properties are only resolvable once the simulation starts playing + self._process_term_cfg_at_play(term_name, term_cfg) + + # set the flag + self._is_scene_entities_resolved = True + + """ + Internal functions. + """ + + def _resolve_common_term_cfg(self, term_name: str, term_cfg: ManagerTermBaseCfg, min_argc: int = 1): + """Resolve common attributes of the term configuration. + + Usually, called by the :meth:`_prepare_terms` method to resolve common attributes of the term + configuration. These include: + + * Resolving the term function and checking if it is callable. + * Checking if the term function's arguments are matched by the parameters. + * Resolving special attributes of the term configuration like ``asset_cfg``, ``sensor_cfg``, etc. + * Initializing the term if it is a class. + + The last two steps are only possible once the simulation starts playing. + + By default, all term functions are expected to have at least one argument, which is the + environment object. Some other managers may expect functions to take more arguments, for + instance, the environment indices as the second argument. In such cases, the + ``min_argc`` argument can be used to specify the minimum number of arguments + required by the term function to be called correctly by the manager. + + Args: + term_name: The name of the term. + term_cfg: The term configuration. + min_argc: The minimum number of arguments required by the term function to be called correctly + by the manager. + + Raises: + TypeError: If the term configuration is not of type :class:`ManagerTermBaseCfg`. + ValueError: If the scene entity defined in the term configuration does not exist. + AttributeError: If the term function is not callable. + ValueError: If the term function's arguments are not matched by the parameters. + """ + # check if the term is a valid term config + if not isinstance(term_cfg, ManagerTermBaseCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type ManagerTermBaseCfg." + f" Received: '{type(term_cfg)}'." + ) + + # get the corresponding function or functional class + if isinstance(term_cfg.func, str): + term_cfg.func = string_to_callable(term_cfg.func) + # check if function is callable + if not callable(term_cfg.func): + raise AttributeError(f"The term '{term_name}' is not callable. Received: {term_cfg.func}") + + # check if the term is a class of valid type + if inspect.isclass(term_cfg.func): + if not issubclass(term_cfg.func, ManagerTermBase): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type ManagerTermBase." + f" Received: '{type(term_cfg.func)}'." + ) + func_static = term_cfg.func.__call__ + min_argc += 1 # forward by 1 to account for 'self' argument + else: + func_static = term_cfg.func + # check if function is callable + if not callable(func_static): + raise AttributeError(f"The term '{term_name}' is not callable. Received: {term_cfg.func}") + + # check statically if the term's arguments are matched by params + term_params = list(term_cfg.params.keys()) + args = inspect.signature(func_static).parameters + args_with_defaults = [arg for arg in args if args[arg].default is not inspect.Parameter.empty] + args_without_defaults = [arg for arg in args if args[arg].default is inspect.Parameter.empty] + args = args_without_defaults + args_with_defaults + # ignore first two arguments for env and env_ids + # Think: Check for cases when kwargs are set inside the function? + if len(args) > min_argc: + if set(args[min_argc:]) != set(term_params + args_with_defaults): + raise ValueError( + f"The term '{term_name}' expects mandatory parameters: {args_without_defaults[min_argc:]}" + f" and optional parameters: {args_with_defaults}, but received: {term_params}." + ) + + # process attributes at runtime + # these properties are only resolvable once the simulation starts playing + if self._env.sim.is_playing(): + self._process_term_cfg_at_play(term_name, term_cfg) + + def _process_term_cfg_at_play(self, term_name: str, term_cfg: ManagerTermBaseCfg): + """Process the term configuration at runtime. + + This function is called when the simulation starts playing. It is used to process the term + configuration at runtime. This includes: + + * Resolving the scene entity configuration for the term. + * Initializing the term if it is a class. + + Since the above steps rely on PhysX to parse over the simulation scene, they are deferred + until the simulation starts playing. + + Args: + term_name: The name of the term. + term_cfg: The term configuration. + """ + for key, value in term_cfg.params.items(): + if isinstance(value, SceneEntityCfg): + # load the entity + try: + value.resolve(self._env.scene) + except ValueError as e: + raise ValueError(f"Error while parsing '{term_name}:{key}'. {e}") + # log the entity for checking later + msg = f"[{term_cfg.__class__.__name__}:{term_name}] Found entity '{value.name}'." + if value.joint_ids is not None: + msg += f"\n\tJoint names: {value.joint_names} [{value.joint_ids}]" + if value.body_ids is not None: + msg += f"\n\tBody names: {value.body_names} [{value.body_ids}]" + # print the information + omni.log.info(msg) + # store the entity + term_cfg.params[key] = value + + # initialize the term if it is a class + if inspect.isclass(term_cfg.func): + omni.log.info(f"Initializing term '{term_name}' with class '{term_cfg.func.__name__}'.") + term_cfg.func = term_cfg.func(cfg=term_cfg, env=self._env) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_term_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_term_cfg.py new file mode 100644 index 00000000000..13ea78851df --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/manager_term_cfg.py @@ -0,0 +1,359 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration terms for different managers.""" + +from __future__ import annotations + +import torch +from collections.abc import Callable +from dataclasses import MISSING +from typing import TYPE_CHECKING, Any + +from isaaclab.utils import configclass +from isaaclab.utils.modifiers import ModifierCfg +from isaaclab.utils.noise import NoiseCfg + +from .scene_entity_cfg import SceneEntityCfg + +if TYPE_CHECKING: + from .action_manager import ActionTerm + from .command_manager import CommandTerm + from .manager_base import ManagerTermBase + from .recorder_manager import RecorderTerm + + +@configclass +class ManagerTermBaseCfg: + """Configuration for a manager term.""" + + func: Callable | ManagerTermBase = MISSING + """The function or class to be called for the term. + + The function must take the environment object as the first argument. + The remaining arguments are specified in the :attr:`params` attribute. + + It also supports `callable classes`_, i.e. classes that implement the :meth:`__call__` + method. In this case, the class should inherit from the :class:`ManagerTermBase` class + and implement the required methods. + + .. _`callable classes`: https://docs.python.org/3/reference/datamodel.html#object.__call__ + """ + + params: dict[str, Any | SceneEntityCfg] = dict() + """The parameters to be passed to the function as keyword arguments. Defaults to an empty dict. + + .. note:: + If the value is a :class:`SceneEntityCfg` object, the manager will query the scene entity + from the :class:`InteractiveScene` and process the entity's joints and bodies as specified + in the :class:`SceneEntityCfg` object. + """ + + +## +# Recorder manager. +## + + +@configclass +class RecorderTermCfg: + """Configuration for an recorder term.""" + + class_type: type[RecorderTerm] = MISSING + """The associated recorder term class. + + The class should inherit from :class:`isaaclab.managers.action_manager.RecorderTerm`. + """ + + +## +# Action manager. +## + + +@configclass +class ActionTermCfg: + """Configuration for an action term.""" + + class_type: type[ActionTerm] = MISSING + """The associated action term class. + + The class should inherit from :class:`isaaclab.managers.action_manager.ActionTerm`. + """ + + asset_name: str = MISSING + """The name of the scene entity. + + This is the name defined in the scene configuration file. See the :class:`InteractiveSceneCfg` + class for more details. + """ + + debug_vis: bool = False + """Whether to visualize debug information. Defaults to False.""" + + clip: dict[str, tuple] | None = None + """Clip range for the action (dict of regex expressions). Defaults to None.""" + + +## +# Command manager. +## + + +@configclass +class CommandTermCfg: + """Configuration for a command generator term.""" + + class_type: type[CommandTerm] = MISSING + """The associated command term class to use. + + The class should inherit from :class:`isaaclab.managers.command_manager.CommandTerm`. + """ + + resampling_time_range: tuple[float, float] = MISSING + """Time before commands are changed [s].""" + debug_vis: bool = False + """Whether to visualize debug information. Defaults to False.""" + + +## +# Curriculum manager. +## + + +@configclass +class CurriculumTermCfg(ManagerTermBaseCfg): + """Configuration for a curriculum term.""" + + func: Callable[..., float | dict[str, float] | None] = MISSING + """The name of the function to be called. + + This function should take the environment object, environment indices + and any other parameters as input and return the curriculum state for + logging purposes. If the function returns None, the curriculum state + is not logged. + """ + + +## +# Observation manager. +## + + +@configclass +class ObservationTermCfg(ManagerTermBaseCfg): + """Configuration for an observation term.""" + + func: Callable[..., torch.Tensor] = MISSING + """The name of the function to be called. + + This function should take the environment object and any other parameters + as input and return the observation signal as torch float tensors of + shape (num_envs, obs_term_dim). + """ + + modifiers: list[ModifierCfg] | None = None + """The list of data modifiers to apply to the observation in order. Defaults to None, + in which case no modifications will be applied. + + Modifiers are applied in the order they are specified in the list. They can be stateless + or stateful, and can be used to apply transformations to the observation data. For example, + a modifier can be used to normalize the observation data or to apply a rolling average. + + For more information on modifiers, see the :class:`~isaaclab.utils.modifiers.ModifierCfg` class. + """ + + noise: NoiseCfg | None = None + """The noise to add to the observation. Defaults to None, in which case no noise is added.""" + + clip: tuple[float, float] | None = None + """The clipping range for the observation after adding noise. Defaults to None, + in which case no clipping is applied.""" + + scale: tuple[float, ...] | float | None = None + """The scale to apply to the observation after clipping. Defaults to None, + in which case no scaling is applied (same as setting scale to :obj:`1`). + + We leverage PyTorch broadcasting to scale the observation tensor with the provided value. If a tuple is provided, + please make sure the length of the tuple matches the dimensions of the tensor outputted from the term. + """ + + history_length: int = 0 + """Number of past observations to store in the observation buffers. Defaults to 0, meaning no history. + + Observation history initializes to empty, but is filled with the first append after reset or initialization. Subsequent history + only adds a single entry to the history buffer. If flatten_history_dim is set to True, the source data of shape + (N, H, D, ...) where N is the batch dimension and H is the history length will be reshaped to a 2D tensor of shape + (N, H*D*...). Otherwise, the data will be returned as is. + """ + + flatten_history_dim: bool = True + """Whether or not the observation manager should flatten history-based observation terms to a 2D (N, D) tensor. + Defaults to True.""" + + +@configclass +class ObservationGroupCfg: + """Configuration for an observation group.""" + + concatenate_terms: bool = True + """Whether to concatenate the observation terms in the group. Defaults to True. + + If true, the observation terms in the group are concatenated along the dimension specified through :attr:`concatenate_dim`. + Otherwise, they are kept separate and returned as a dictionary. + + If the observation group contains terms of different dimensions, it must be set to False. + """ + + concatenate_dim: int = -1 + """Dimension along to concatenate the different observation terms. Defaults to -1, which + means the last dimension of the observation terms. + + If :attr:`concatenate_terms` is True, this parameter specifies the dimension along which the observation terms are concatenated. + The indicated dimension depends on the shape of the observations. For instance, for a 2D RGB image of shape (H, W, C), the dimension + 0 means concatenating along the height, 1 along the width, and 2 along the channels. The offset due + to the batched environment is handled automatically. + """ + + enable_corruption: bool = False + """Whether to enable corruption for the observation group. Defaults to False. + + If true, the observation terms in the group are corrupted by adding noise (if specified). + Otherwise, no corruption is applied. + """ + + history_length: int | None = None + """Number of past observation to store in the observation buffers for all observation terms in group. + + This parameter will override :attr:`ObservationTermCfg.history_length` if set. Defaults to None. If None, each + terms history will be controlled on a per term basis. See :class:`ObservationTermCfg` for details on history_length + implementation. + """ + + flatten_history_dim: bool = True + """Flag to flatten history-based observation terms to a 2D (num_env, D) tensor for all observation terms in group. + Defaults to True. + + This parameter will override all :attr:`ObservationTermCfg.flatten_history_dim` in the group if + ObservationGroupCfg.history_length is set. + """ + + +## +# Event manager +## + + +@configclass +class EventTermCfg(ManagerTermBaseCfg): + """Configuration for a event term.""" + + func: Callable[..., None] = MISSING + """The name of the function to be called. + + This function should take the environment object, environment indices + and any other parameters as input. + """ + + mode: str = MISSING + """The mode in which the event term is applied. + + Note: + The mode name ``"interval"`` is a special mode that is handled by the + manager Hence, its name is reserved and cannot be used for other modes. + """ + + interval_range_s: tuple[float, float] | None = None + """The range of time in seconds at which the term is applied. Defaults to None. + + Based on this, the interval is sampled uniformly between the specified + range for each environment instance. The term is applied on the environment + instances where the current time hits the interval time. + + Note: + This is only used if the mode is ``"interval"``. + """ + + is_global_time: bool = False + """Whether randomization should be tracked on a per-environment basis. Defaults to False. + + If True, the same interval time is used for all the environment instances. + If False, the interval time is sampled independently for each environment instance + and the term is applied when the current time hits the interval time for that instance. + + Note: + This is only used if the mode is ``"interval"``. + """ + + min_step_count_between_reset: int = 0 + """The number of environment steps after which the term is applied since its last application. Defaults to 0. + + When the mode is "reset", the term is only applied if the number of environment steps since + its last application exceeds this quantity. This helps to avoid calling the term too often, + thereby improving performance. + + If the value is zero, the term is applied on every call to the manager with the mode "reset". + + Note: + This is only used if the mode is ``"reset"``. + """ + + +## +# Reward manager. +## + + +@configclass +class RewardTermCfg(ManagerTermBaseCfg): + """Configuration for a reward term.""" + + func: Callable[..., torch.Tensor] = MISSING + """The name of the function to be called. + + This function should take the environment object and any other parameters + as input and return the reward signals as torch float tensors of + shape (num_envs,). + """ + + weight: float = MISSING + """The weight of the reward term. + + This is multiplied with the reward term's value to compute the final + reward. + + Note: + If the weight is zero, the reward term is ignored. + """ + + +## +# Termination manager. +## + + +@configclass +class TerminationTermCfg(ManagerTermBaseCfg): + """Configuration for a termination term.""" + + func: Callable[..., torch.Tensor] = MISSING + """The name of the function to be called. + + This function should take the environment object and any other parameters + as input and return the termination signals as torch boolean tensors of + shape (num_envs,). + """ + + time_out: bool = False + """Whether the termination term contributes towards episodic timeouts. Defaults to False. + + Note: + These usually correspond to tasks that have a fixed time limit. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/observation_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/observation_manager.py new file mode 100644 index 00000000000..1af30442c7d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/observation_manager.py @@ -0,0 +1,554 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Observation manager for computing observation signals for a given world.""" + +from __future__ import annotations + +import inspect +import numpy as np +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +from isaaclab.utils import class_to_dict, modifiers +from isaaclab.utils.buffers import CircularBuffer + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import ObservationGroupCfg, ObservationTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class ObservationManager(ManagerBase): + """Manager for computing observation signals for a given world. + + Observations are organized into groups based on their intended usage. This allows having different observation + groups for different types of learning such as asymmetric actor-critic and student-teacher training. Each + group contains observation terms which contain information about the observation function to call, the noise + corruption model to use, and the sensor to retrieve data from. + + Each observation group should inherit from the :class:`ObservationGroupCfg` class. Within each group, each + observation term should instantiate the :class:`ObservationTermCfg` class. Based on the configuration, the + observations in a group can be concatenated into a single tensor or returned as a dictionary with keys + corresponding to the term's name. + + If the observations in a group are concatenated, the shape of the concatenated tensor is computed based on the + shapes of the individual observation terms. This information is stored in the :attr:`group_obs_dim` dictionary + with keys as the group names and values as the shape of the observation tensor. When the terms in a group are not + concatenated, the attribute stores a list of shapes for each term in the group. + + .. note:: + When the observation terms in a group do not have the same shape, the observation terms cannot be + concatenated. In this case, please set the :attr:`ObservationGroupCfg.concatenate_terms` attribute in the + group configuration to False. + + Observations can also have history. This means a running history is updated per sim step. History can be controlled + per :class:`ObservationTermCfg` (See the :attr:`ObservationTermCfg.history_length` and + :attr:`ObservationTermCfg.flatten_history_dim`). History can also be controlled via :class:`ObservationGroupCfg` + where group configuration overwrites per term configuration if set. History follows an oldest to newest ordering. + + The observation manager can be used to compute observations for all the groups or for a specific group. The + observations are computed by calling the registered functions for each term in the group. The functions are + called in the order of the terms in the group. The functions are expected to return a tensor with shape + (num_envs, ...). + + If a noise model or custom modifier is registered for a term, the function is called to corrupt + the observation. The corruption function is expected to return a tensor with the same shape as the observation. + The observations are clipped and scaled as per the configuration settings. + """ + + def __init__(self, cfg: object, env: ManagerBasedEnv): + """Initialize observation manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, ObservationGroupCfg]``). + env: The environment instance. + + Raises: + ValueError: If the configuration is None. + RuntimeError: If the shapes of the observation terms in a group are not compatible for concatenation + and the :attr:`~ObservationGroupCfg.concatenate_terms` attribute is set to True. + """ + # check that cfg is not None + if cfg is None: + raise ValueError("Observation manager configuration is None. Please provide a valid configuration.") + + # call the base class constructor (this will parse the terms config) + super().__init__(cfg, env) + + # compute combined vector for obs group + self._group_obs_dim: dict[str, tuple[int, ...] | list[tuple[int, ...]]] = dict() + for group_name, group_term_dims in self._group_obs_term_dim.items(): + # if terms are concatenated, compute the combined shape into a single tuple + # otherwise, keep the list of shapes as is + if self._group_obs_concatenate[group_name]: + try: + term_dims = torch.stack([torch.tensor(dims, device="cpu") for dims in group_term_dims], dim=0) + if len(term_dims.shape) > 1: + if self._group_obs_concatenate_dim[group_name] >= 0: + dim = self._group_obs_concatenate_dim[group_name] - 1 # account for the batch offset + else: + dim = self._group_obs_concatenate_dim[group_name] + dim_sum = torch.sum(term_dims[:, dim], dim=0) + term_dims[0, dim] = dim_sum + term_dims = term_dims[0] + else: + term_dims = torch.sum(term_dims, dim=0) + self._group_obs_dim[group_name] = tuple(term_dims.tolist()) + except RuntimeError: + raise RuntimeError( + f"Unable to concatenate observation terms in group '{group_name}'." + f" The shapes of the terms are: {group_term_dims}." + " Please ensure that the shapes are compatible for concatenation." + " Otherwise, set 'concatenate_terms' to False in the group configuration." + ) + else: + self._group_obs_dim[group_name] = group_term_dims + + # Stores the latest observations. + self._obs_buffer: dict[str, torch.Tensor | dict[str, torch.Tensor]] | None = None + + def __str__(self) -> str: + """Returns: A string representation for the observation manager.""" + msg = f" contains {len(self._group_obs_term_names)} groups.\n" + + # add info for each group + for group_name, group_dim in self._group_obs_dim.items(): + # create table for term information + table = PrettyTable() + table.title = f"Active Observation Terms in Group: '{group_name}'" + if self._group_obs_concatenate[group_name]: + table.title += f" (shape: {group_dim})" + table.field_names = ["Index", "Name", "Shape"] + # set alignment of table columns + table.align["Name"] = "l" + # add info for each term + obs_terms = zip( + self._group_obs_term_names[group_name], + self._group_obs_term_dim[group_name], + ) + for index, (name, dims) in enumerate(obs_terms): + # resolve inputs to simplify prints + tab_dims = tuple(dims) + # add row + table.add_row([index, name, tab_dims]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + terms = [] + + if self._obs_buffer is None: + self.compute() + obs_buffer: dict[str, torch.Tensor | dict[str, torch.Tensor]] = self._obs_buffer + + for group_name, _ in self._group_obs_dim.items(): + if not self.group_obs_concatenate[group_name]: + for name, term in obs_buffer[group_name].items(): + terms.append((group_name + "-" + name, term[env_idx].cpu().tolist())) + continue + + idx = 0 + # add info for each term + data = obs_buffer[group_name] + for name, shape in zip( + self._group_obs_term_names[group_name], + self._group_obs_term_dim[group_name], + ): + data_length = np.prod(shape) + term = data[env_idx, idx : idx + data_length] + terms.append((group_name + "-" + name, term.cpu().tolist())) + idx += data_length + + return terms + + """ + Properties. + """ + + @property + def active_terms(self) -> dict[str, list[str]]: + """Name of active observation terms in each group. + + The keys are the group names and the values are the list of observation term names in the group. + """ + return self._group_obs_term_names + + @property + def group_obs_dim(self) -> dict[str, tuple[int, ...] | list[tuple[int, ...]]]: + """Shape of computed observations in each group. + + The key is the group name and the value is the shape of the observation tensor. + If the terms in the group are concatenated, the value is a single tuple representing the + shape of the concatenated observation tensor. Otherwise, the value is a list of tuples, + where each tuple represents the shape of the observation tensor for a term in the group. + """ + return self._group_obs_dim + + @property + def group_obs_term_dim(self) -> dict[str, list[tuple[int, ...]]]: + """Shape of individual observation terms in each group. + + The key is the group name and the value is a list of tuples representing the shape of the observation terms + in the group. The order of the tuples corresponds to the order of the terms in the group. + This matches the order of the terms in the :attr:`active_terms`. + """ + return self._group_obs_term_dim + + @property + def group_obs_concatenate(self) -> dict[str, bool]: + """Whether the observation terms are concatenated in each group or not. + + The key is the group name and the value is a boolean specifying whether the observation terms in the group + are concatenated into a single tensor. If True, the observations are concatenated along the last dimension. + + The values are set based on the :attr:`~ObservationGroupCfg.concatenate_terms` attribute in the group + configuration. + """ + return self._group_obs_concatenate + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + # call all terms that are classes + for group_name, group_cfg in self._group_obs_class_term_cfgs.items(): + for term_cfg in group_cfg: + term_cfg.func.reset(env_ids=env_ids) + # reset terms with history + for term_name in self._group_obs_term_names[group_name]: + if term_name in self._group_obs_term_history_buffer[group_name]: + self._group_obs_term_history_buffer[group_name][term_name].reset(batch_ids=env_ids) + # call all modifiers that are classes + for mod in self._group_obs_class_modifiers: + mod.reset(env_ids=env_ids) + + # nothing to log here + return {} + + def compute(self) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: + """Compute the observations per group for all groups. + + The method computes the observations for all the groups handled by the observation manager. + Please check the :meth:`compute_group` on the processing of observations per group. + + Returns: + A dictionary with keys as the group names and values as the computed observations. + The observations are either concatenated into a single tensor or returned as a dictionary + with keys corresponding to the term's name. + """ + # create a buffer for storing obs from all the groups + obs_buffer = dict() + # iterate over all the terms in each group + for group_name in self._group_obs_term_names: + obs_buffer[group_name] = self.compute_group(group_name) + # otherwise return a dict with observations of all groups + + # Cache the observations. + self._obs_buffer = obs_buffer + return obs_buffer + + def compute_group(self, group_name: str) -> torch.Tensor | dict[str, torch.Tensor]: + """Computes the observations for a given group. + + The observations for a given group are computed by calling the registered functions for each + term in the group. The functions are called in the order of the terms in the group. The functions + are expected to return a tensor with shape (num_envs, ...). + + The following steps are performed for each observation term: + + 1. Compute observation term by calling the function + 2. Apply custom modifiers in the order specified in :attr:`ObservationTermCfg.modifiers` + 3. Apply corruption/noise model based on :attr:`ObservationTermCfg.noise` + 4. Apply clipping based on :attr:`ObservationTermCfg.clip` + 5. Apply scaling based on :attr:`ObservationTermCfg.scale` + + We apply noise to the computed term first to maintain the integrity of how noise affects the data + as it truly exists in the real world. If the noise is applied after clipping or scaling, the noise + could be artificially constrained or amplified, which might misrepresent how noise naturally occurs + in the data. + + Args: + group_name: The name of the group for which to compute the observations. Defaults to None, + in which case observations for all the groups are computed and returned. + + Returns: + Depending on the group's configuration, the tensors for individual observation terms are + concatenated along the last dimension into a single tensor. Otherwise, they are returned as + a dictionary with keys corresponding to the term's name. + + Raises: + ValueError: If input ``group_name`` is not a valid group handled by the manager. + """ + # check ig group name is valid + if group_name not in self._group_obs_term_names: + raise ValueError( + f"Unable to find the group '{group_name}' in the observation manager." + f" Available groups are: {list(self._group_obs_term_names.keys())}" + ) + # iterate over all the terms in each group + group_term_names = self._group_obs_term_names[group_name] + # buffer to store obs per group + group_obs = dict.fromkeys(group_term_names, None) + # read attributes for each term + obs_terms = zip(group_term_names, self._group_obs_term_cfgs[group_name]) + + # evaluate terms: compute, add noise, clip, scale, custom modifiers + for term_name, term_cfg in obs_terms: + # compute term's value + obs: torch.Tensor = term_cfg.func(self._env, **term_cfg.params).clone() + # apply post-processing + if term_cfg.modifiers is not None: + for modifier in term_cfg.modifiers: + obs = modifier.func(obs, **modifier.params) + if term_cfg.noise: + obs = term_cfg.noise.func(obs, term_cfg.noise) + if term_cfg.clip: + obs = obs.clip_(min=term_cfg.clip[0], max=term_cfg.clip[1]) + if term_cfg.scale is not None: + obs = obs.mul_(term_cfg.scale) + # Update the history buffer if observation term has history enabled + if term_cfg.history_length > 0: + self._group_obs_term_history_buffer[group_name][term_name].append(obs) + if term_cfg.flatten_history_dim: + group_obs[term_name] = self._group_obs_term_history_buffer[group_name][term_name].buffer.reshape( + self._env.num_envs, -1 + ) + else: + group_obs[term_name] = self._group_obs_term_history_buffer[group_name][term_name].buffer + else: + group_obs[term_name] = obs + + # concatenate all observations in the group together + if self._group_obs_concatenate[group_name]: + # set the concatenate dimension, account for the batch dimension if positive dimension is given + return torch.cat(list(group_obs.values()), dim=self._group_obs_concatenate_dim[group_name]) + else: + return group_obs + + def serialize(self) -> dict: + """Serialize the observation term configurations for all active groups. + + Returns: + A dictionary where each group name maps to its serialized observation term configurations. + """ + output = { + group_name: { + term_name: ( + term_cfg.func.serialize() + if isinstance(term_cfg.func, ManagerTermBase) + else {"cfg": class_to_dict(term_cfg)} + ) + for term_name, term_cfg in zip( + self._group_obs_term_names[group_name], + self._group_obs_term_cfgs[group_name], + ) + } + for group_name in self.active_terms.keys() + } + + return output + + """ + Helper functions. + """ + + def _prepare_terms(self): + """Prepares a list of observation terms functions.""" + # create buffers to store information for each observation group + # TODO: Make this more convenient by using data structures. + self._group_obs_term_names: dict[str, list[str]] = dict() + self._group_obs_term_dim: dict[str, list[tuple[int, ...]]] = dict() + self._group_obs_term_cfgs: dict[str, list[ObservationTermCfg]] = dict() + self._group_obs_class_term_cfgs: dict[str, list[ObservationTermCfg]] = dict() + self._group_obs_concatenate: dict[str, bool] = dict() + self._group_obs_concatenate_dim: dict[str, int] = dict() + + self._group_obs_term_history_buffer: dict[str, dict] = dict() + # create a list to store modifiers that are classes + # we store it as a separate list to only call reset on them and prevent unnecessary calls + self._group_obs_class_modifiers: list[modifiers.ModifierBase] = list() + + # make sure the simulation is playing since we compute obs dims which needs asset quantities + if not self._env.sim.is_playing(): + raise RuntimeError( + "Simulation is not playing. Observation manager requires the simulation to be playing" + " to compute observation dimensions. Please start the simulation before using the" + " observation manager." + ) + + # check if config is dict already + if isinstance(self.cfg, dict): + group_cfg_items = self.cfg.items() + else: + group_cfg_items = self.cfg.__dict__.items() + # iterate over all the groups + for group_name, group_cfg in group_cfg_items: + # check for non config + if group_cfg is None: + continue + # check if the term is a curriculum term + if not isinstance(group_cfg, ObservationGroupCfg): + raise TypeError( + f"Observation group '{group_name}' is not of type 'ObservationGroupCfg'." + f" Received: '{type(group_cfg)}'." + ) + # initialize list for the group settings + self._group_obs_term_names[group_name] = list() + self._group_obs_term_dim[group_name] = list() + self._group_obs_term_cfgs[group_name] = list() + self._group_obs_class_term_cfgs[group_name] = list() + group_entry_history_buffer: dict[str, CircularBuffer] = dict() + # read common config for the group + self._group_obs_concatenate[group_name] = group_cfg.concatenate_terms + self._group_obs_concatenate_dim[group_name] = ( + group_cfg.concatenate_dim + 1 if group_cfg.concatenate_dim >= 0 else group_cfg.concatenate_dim + ) + # check if config is dict already + if isinstance(group_cfg, dict): + group_cfg_items = group_cfg.items() + else: + group_cfg_items = group_cfg.__dict__.items() + # iterate over all the terms in each group + for term_name, term_cfg in group_cfg_items: + # skip non-obs settings + if term_name in [ + "enable_corruption", + "concatenate_terms", + "history_length", + "flatten_history_dim", + "concatenate_dim", + ]: + continue + # check for non config + if term_cfg is None: + continue + if not isinstance(term_cfg, ObservationTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type ObservationTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # resolve common terms in the config + self._resolve_common_term_cfg(f"{group_name}/{term_name}", term_cfg, min_argc=1) + + # check noise settings + if not group_cfg.enable_corruption: + term_cfg.noise = None + # check group history params and override terms + if group_cfg.history_length is not None: + term_cfg.history_length = group_cfg.history_length + term_cfg.flatten_history_dim = group_cfg.flatten_history_dim + # add term config to list to list + self._group_obs_term_names[group_name].append(term_name) + self._group_obs_term_cfgs[group_name].append(term_cfg) + + # call function the first time to fill up dimensions + obs_dims = tuple(term_cfg.func(self._env, **term_cfg.params).shape) + + # if scale is set, check if single float or tuple + if term_cfg.scale is not None: + if not isinstance(term_cfg.scale, (float, int, tuple)): + raise TypeError( + f"Scale for observation term '{term_name}' in group '{group_name}'" + f" is not of type float, int or tuple. Received: '{type(term_cfg.scale)}'." + ) + if isinstance(term_cfg.scale, tuple) and len(term_cfg.scale) != obs_dims[1]: + raise ValueError( + f"Scale for observation term '{term_name}' in group '{group_name}'" + f" does not match the dimensions of the observation. Expected: {obs_dims[1]}" + f" but received: {len(term_cfg.scale)}." + ) + + # cast the scale into torch tensor + term_cfg.scale = torch.tensor(term_cfg.scale, dtype=torch.float, device=self._env.device) + + # prepare modifiers for each observation + if term_cfg.modifiers is not None: + # initialize list of modifiers for term + for mod_cfg in term_cfg.modifiers: + # check if class modifier and initialize with observation size when adding + if isinstance(mod_cfg, modifiers.ModifierCfg): + # to list of modifiers + if inspect.isclass(mod_cfg.func): + if not issubclass(mod_cfg.func, modifiers.ModifierBase): + raise TypeError( + f"Modifier function '{mod_cfg.func}' for observation term '{term_name}'" + f" is not a subclass of 'ModifierBase'. Received: '{type(mod_cfg.func)}'." + ) + mod_cfg.func = mod_cfg.func(cfg=mod_cfg, data_dim=obs_dims, device=self._env.device) + + # add to list of class modifiers + self._group_obs_class_modifiers.append(mod_cfg.func) + else: + raise TypeError( + f"Modifier configuration '{mod_cfg}' of observation term '{term_name}' is not of" + f" required type ModifierCfg, Received: '{type(mod_cfg)}'" + ) + + # check if function is callable + if not callable(mod_cfg.func): + raise AttributeError( + f"Modifier '{mod_cfg}' of observation term '{term_name}' is not callable." + f" Received: {mod_cfg.func}" + ) + + # check if term's arguments are matched by params + term_params = list(mod_cfg.params.keys()) + args = inspect.signature(mod_cfg.func).parameters + args_with_defaults = [arg for arg in args if args[arg].default is not inspect.Parameter.empty] + args_without_defaults = [arg for arg in args if args[arg].default is inspect.Parameter.empty] + args = args_without_defaults + args_with_defaults + # ignore first two arguments for env and env_ids + # Think: Check for cases when kwargs are set inside the function? + if len(args) > 1: + if set(args[1:]) != set(term_params + args_with_defaults): + raise ValueError( + f"Modifier '{mod_cfg}' of observation term '{term_name}' expects" + f" mandatory parameters: {args_without_defaults[1:]}" + f" and optional parameters: {args_with_defaults}, but received: {term_params}." + ) + + # create history buffers and calculate history term dimensions + if term_cfg.history_length > 0: + group_entry_history_buffer[term_name] = CircularBuffer( + max_len=term_cfg.history_length, batch_size=self._env.num_envs, device=self._env.device + ) + old_dims = list(obs_dims) + old_dims.insert(1, term_cfg.history_length) + obs_dims = tuple(old_dims) + if term_cfg.flatten_history_dim: + obs_dims = (obs_dims[0], np.prod(obs_dims[1:])) + + self._group_obs_term_dim[group_name].append(obs_dims[1:]) + + # add term in a separate list if term is a class + if isinstance(term_cfg.func, ManagerTermBase): + self._group_obs_class_term_cfgs[group_name].append(term_cfg) + # call reset (in-case above call to get obs dims changed the state) + term_cfg.func.reset() + # add history buffers for each group + self._group_obs_term_history_buffer[group_name] = group_entry_history_buffer diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/recorder_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/recorder_manager.py new file mode 100644 index 00000000000..40807b4ca7a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/recorder_manager.py @@ -0,0 +1,502 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Recorder manager for recording data produced from the given world.""" + +from __future__ import annotations + +import enum +import os +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass +from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import RecorderTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class DatasetExportMode(enum.IntEnum): + """The mode to handle episode exports.""" + + EXPORT_NONE = 0 # Export none of the episodes + EXPORT_ALL = 1 # Export all episodes to a single dataset file + EXPORT_SUCCEEDED_FAILED_IN_SEPARATE_FILES = 2 # Export succeeded and failed episodes in separate files + EXPORT_SUCCEEDED_ONLY = 3 # Export only succeeded episodes to a single dataset file + + +@configclass +class RecorderManagerBaseCfg: + """Base class for configuring recorder manager terms.""" + + dataset_file_handler_class_type: type = HDF5DatasetFileHandler + + dataset_export_dir_path: str = "/tmp/isaaclab/logs" + """The directory path where the recorded datasets are exported.""" + + dataset_filename: str = "dataset" + """Dataset file name without file extension.""" + + dataset_export_mode: DatasetExportMode = DatasetExportMode.EXPORT_ALL + """The mode to handle episode exports.""" + + export_in_record_pre_reset: bool = True + """Whether to export episodes in the record_pre_reset call.""" + + +class RecorderTerm(ManagerTermBase): + """Base class for recorder terms. + + The recorder term is responsible for recording data at various stages of the environment's lifecycle. + A recorder term is comprised of four user-defined callbacks to record data in the corresponding stages: + + * Pre-reset recording: This callback is invoked at the beginning of `env.reset()` before the reset is effective. + * Post-reset recording: This callback is invoked at the end of `env.reset()`. + * Pre-step recording: This callback is invoked at the beginning of `env.step()`, after the step action is processed + and before the action is applied by the action manager. + * Post-step recording: This callback is invoked at the end of `env.step()` when all the managers are processed. + """ + + def __init__(self, cfg: RecorderTermCfg, env: ManagerBasedEnv): + """Initialize the recorder term. + + Args: + cfg: The configuration object. + env: The environment instance. + """ + # call the base class constructor + super().__init__(cfg, env) + + """ + User-defined callbacks. + """ + + def record_pre_reset(self, env_ids: Sequence[int] | None) -> tuple[str | None, torch.Tensor | dict | None]: + """Record data at the beginning of env.reset() before reset is effective. + + Args: + env_ids: The environment ids. All environments should be considered when set to None. + + Returns: + A tuple of key and value to be recorded. + The key can contain nested keys separated by '/'. For example, "obs/joint_pos" would add the given + value under ['obs']['policy'] in the underlying dictionary in the recorded episode data. + The value can be a tensor or a nested dictionary of tensors. The shape of a tensor in the value + is (env_ids, ...). + """ + return None, None + + def record_post_reset(self, env_ids: Sequence[int] | None) -> tuple[str | None, torch.Tensor | dict | None]: + """Record data at the end of env.reset(). + + Args: + env_ids: The environment ids. All environments should be considered when set to None. + + Returns: + A tuple of key and value to be recorded. + Please refer to the `record_pre_reset` function for more details. + """ + return None, None + + def record_pre_step(self) -> tuple[str | None, torch.Tensor | dict | None]: + """Record data in the beginning of env.step() after action is cached/processed in the ActionManager. + + Returns: + A tuple of key and value to be recorded. + Please refer to the `record_pre_reset` function for more details. + """ + return None, None + + def record_post_step(self) -> tuple[str | None, torch.Tensor | dict | None]: + """Record data at the end of env.step() when all the managers are processed. + + Returns: + A tuple of key and value to be recorded. + Please refer to the `record_pre_reset` function for more details. + """ + return None, None + + +class RecorderManager(ManagerBase): + """Manager for recording data from recorder terms.""" + + def __init__(self, cfg: object, env: ManagerBasedEnv): + """Initialize the recorder manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, RecorderTermCfg]``). + env: The environment instance. + """ + self._term_names: list[str] = list() + self._terms: dict[str, RecorderTerm] = dict() + + # Do nothing if cfg is None or an empty dict + if not cfg: + return + + super().__init__(cfg, env) + + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + if not isinstance(cfg, RecorderManagerBaseCfg): + raise TypeError("Configuration for the recorder manager is not of type RecorderManagerBaseCfg.") + + # create episode data buffer indexed by environment id + self._episodes: dict[int, EpisodeData] = dict() + for env_id in range(env.num_envs): + self._episodes[env_id] = EpisodeData() + + env_name = getattr(env.cfg, "env_name", None) + + self._dataset_file_handler = None + if cfg.dataset_export_mode != DatasetExportMode.EXPORT_NONE: + self._dataset_file_handler = cfg.dataset_file_handler_class_type() + self._dataset_file_handler.create( + os.path.join(cfg.dataset_export_dir_path, cfg.dataset_filename), env_name=env_name + ) + + self._failed_episode_dataset_file_handler = None + if cfg.dataset_export_mode == DatasetExportMode.EXPORT_SUCCEEDED_FAILED_IN_SEPARATE_FILES: + self._failed_episode_dataset_file_handler = cfg.dataset_file_handler_class_type() + self._failed_episode_dataset_file_handler.create( + os.path.join(cfg.dataset_export_dir_path, f"{cfg.dataset_filename}_failed"), env_name=env_name + ) + + self._exported_successful_episode_count = {} + self._exported_failed_episode_count = {} + + def __str__(self) -> str: + """Returns: A string representation for recorder manager.""" + msg = f" contains {len(self._term_names)} active terms.\n" + # create table for term information + table = PrettyTable() + table.title = "Active Recorder Terms" + table.field_names = ["Index", "Name"] + # set alignment of table columns + table.align["Name"] = "l" + # add info on each term + for index, name in enumerate(self._term_names): + table.add_row([index, name]) + # convert table to string + msg += table.get_string() + msg += "\n" + return msg + + def __del__(self): + """Destructor for recorder.""" + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + if self._dataset_file_handler is not None: + self._dataset_file_handler.close() + + if self._failed_episode_dataset_file_handler is not None: + self._failed_episode_dataset_file_handler.close() + + """ + Properties. + """ + + @property + def active_terms(self) -> list[str]: + """Name of active recorder terms.""" + return self._term_names + + @property + def exported_successful_episode_count(self, env_id=None) -> int: + """Number of successful episodes. + + Args: + env_id: The environment id. Defaults to None, in which case all environments are considered. + + Returns: + The number of successful episodes. + """ + if not hasattr(self, "_exported_successful_episode_count"): + return 0 + if env_id is not None: + return self._exported_successful_episode_count.get(env_id, 0) + return sum(self._exported_successful_episode_count.values()) + + @property + def exported_failed_episode_count(self, env_id=None) -> int: + """Number of failed episodes. + + Args: + env_id: The environment id. Defaults to None, in which case all environments are considered. + + Returns: + The number of failed episodes. + """ + if not hasattr(self, "_exported_failed_episode_count"): + return 0 + if env_id is not None: + return self._exported_failed_episode_count.get(env_id, 0) + return sum(self._exported_failed_episode_count.values()) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Resets the recorder data. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + + Returns: + An empty dictionary. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return {} + + # resolve environment ids + if env_ids is None: + env_ids = list(range(self._env.num_envs)) + if isinstance(env_ids, torch.Tensor): + env_ids = env_ids.tolist() + + for term in self._terms.values(): + term.reset(env_ids=env_ids) + + for env_id in env_ids: + self._episodes[env_id] = EpisodeData() + + # nothing to log here + return {} + + def get_episode(self, env_id: int) -> EpisodeData: + """Returns the episode data for the given environment id. + + Args: + env_id: The environment id. + + Returns: + The episode data for the given environment id. + """ + return self._episodes.get(env_id, EpisodeData()) + + def add_to_episodes(self, key: str, value: torch.Tensor | dict, env_ids: Sequence[int] | None = None): + """Adds the given key-value pair to the episodes for the given environment ids. + + Args: + key: The key of the given value to be added to the episodes. The key can contain nested keys + separated by '/'. For example, "obs/joint_pos" would add the given value under ['obs']['policy'] + in the underlying dictionary in the episode data. + value: The value to be added to the episodes. The value can be a tensor or a nested dictionary of tensors. + The shape of a tensor in the value is (env_ids, ...). + env_ids: The environment ids. Defaults to None, in which case all environments are considered. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + # resolve environment ids + if key is None: + return + if env_ids is None: + env_ids = list(range(self._env.num_envs)) + if isinstance(env_ids, torch.Tensor): + env_ids = env_ids.tolist() + + if isinstance(value, dict): + for sub_key, sub_value in value.items(): + self.add_to_episodes(f"{key}/{sub_key}", sub_value, env_ids) + return + + for value_index, env_id in enumerate(env_ids): + if env_id not in self._episodes: + self._episodes[env_id] = EpisodeData() + self._episodes[env_id].env_id = env_id + self._episodes[env_id].add(key, value[value_index]) + + def set_success_to_episodes(self, env_ids: Sequence[int] | None, success_values: torch.Tensor): + """Sets the task success values to the episodes for the given environment ids. + + Args: + env_ids: The environment ids. Defaults to None, in which case all environments are considered. + success_values: The task success values to be set to the episodes. The shape of the tensor is (env_ids, 1). + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + # resolve environment ids + if env_ids is None: + env_ids = list(range(self._env.num_envs)) + if isinstance(env_ids, torch.Tensor): + env_ids = env_ids.tolist() + + for value_index, env_id in enumerate(env_ids): + self._episodes[env_id].success = success_values[value_index].item() + + def record_pre_step(self) -> None: + """Trigger recorder terms for pre-step functions.""" + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + for term in self._terms.values(): + key, value = term.record_pre_step() + self.add_to_episodes(key, value) + + def record_post_step(self) -> None: + """Trigger recorder terms for post-step functions.""" + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + for term in self._terms.values(): + key, value = term.record_post_step() + self.add_to_episodes(key, value) + + def record_pre_reset(self, env_ids: Sequence[int] | None, force_export_or_skip=None) -> None: + """Trigger recorder terms for pre-reset functions. + + Args: + env_ids: The environment ids in which a reset is triggered. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + if env_ids is None: + env_ids = list(range(self._env.num_envs)) + if isinstance(env_ids, torch.Tensor): + env_ids = env_ids.tolist() + + for term in self._terms.values(): + key, value = term.record_pre_reset(env_ids) + self.add_to_episodes(key, value, env_ids) + + # Set task success values for the relevant episodes + success_results = torch.zeros(len(env_ids), dtype=bool, device=self._env.device) + # Check success indicator from termination terms + if hasattr(self._env, "termination_manager"): + if "success" in self._env.termination_manager.active_terms: + success_results |= self._env.termination_manager.get_term("success")[env_ids] + self.set_success_to_episodes(env_ids, success_results) + + if force_export_or_skip or (force_export_or_skip is None and self.cfg.export_in_record_pre_reset): + self.export_episodes(env_ids) + + def record_post_reset(self, env_ids: Sequence[int] | None) -> None: + """Trigger recorder terms for post-reset functions. + + Args: + env_ids: The environment ids in which a reset is triggered. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + for term in self._terms.values(): + key, value = term.record_post_reset(env_ids) + self.add_to_episodes(key, value, env_ids) + + def export_episodes(self, env_ids: Sequence[int] | None = None) -> None: + """Concludes and exports the episodes for the given environment ids. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + """ + # Do nothing if no active recorder terms are provided + if len(self.active_terms) == 0: + return + + if env_ids is None: + env_ids = list(range(self._env.num_envs)) + if isinstance(env_ids, torch.Tensor): + env_ids = env_ids.tolist() + + # Export episode data through dataset exporter + need_to_flush = False + for env_id in env_ids: + if env_id in self._episodes and not self._episodes[env_id].is_empty(): + episode_succeeded = self._episodes[env_id].success + target_dataset_file_handler = None + if (self.cfg.dataset_export_mode == DatasetExportMode.EXPORT_ALL) or ( + self.cfg.dataset_export_mode == DatasetExportMode.EXPORT_SUCCEEDED_ONLY and episode_succeeded + ): + target_dataset_file_handler = self._dataset_file_handler + elif self.cfg.dataset_export_mode == DatasetExportMode.EXPORT_SUCCEEDED_FAILED_IN_SEPARATE_FILES: + if episode_succeeded: + target_dataset_file_handler = self._dataset_file_handler + else: + target_dataset_file_handler = self._failed_episode_dataset_file_handler + if target_dataset_file_handler is not None: + target_dataset_file_handler.write_episode(self._episodes[env_id]) + need_to_flush = True + # Update episode count + if episode_succeeded: + self._exported_successful_episode_count[env_id] = ( + self._exported_successful_episode_count.get(env_id, 0) + 1 + ) + else: + self._exported_failed_episode_count[env_id] = self._exported_failed_episode_count.get(env_id, 0) + 1 + # Reset the episode buffer for the given environment after export + self._episodes[env_id] = EpisodeData() + + if need_to_flush: + if self._dataset_file_handler is not None: + self._dataset_file_handler.flush() + if self._failed_episode_dataset_file_handler is not None: + self._failed_episode_dataset_file_handler.flush() + + """ + Helper functions. + """ + + def _prepare_terms(self): + """Prepares a list of recorder terms.""" + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + for term_name, term_cfg in cfg_items: + # skip non-term settings + if term_name in [ + "dataset_file_handler_class_type", + "dataset_filename", + "dataset_export_dir_path", + "dataset_export_mode", + "export_in_record_pre_reset", + ]: + continue + # check if term config is None + if term_cfg is None: + continue + # check valid type + if not isinstance(term_cfg, RecorderTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type RecorderTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # create the recorder term + term = term_cfg.class_type(term_cfg, self._env) + # sanity check if term is valid type + if not isinstance(term, RecorderTerm): + raise TypeError(f"Returned object for the term '{term_name}' is not of type RecorderTerm.") + # add term name and parameters + self._term_names.append(term_name) + self._terms[term_name] = term diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/reward_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/reward_manager.py new file mode 100644 index 00000000000..b9791b9d9b2 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/reward_manager.py @@ -0,0 +1,251 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Reward manager for computing reward signals for a given world.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import RewardTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class RewardManager(ManagerBase): + """Manager for computing reward signals for a given world. + + The reward manager computes the total reward as a sum of the weighted reward terms. The reward + terms are parsed from a nested config class containing the reward manger's settings and reward + terms configuration. + + The reward terms are parsed from a config class containing the manager's settings and each term's + parameters. Each reward term should instantiate the :class:`RewardTermCfg` class. + + .. note:: + + The reward manager multiplies the reward term's ``weight`` with the time-step interval ``dt`` + of the environment. This is done to ensure that the computed reward terms are balanced with + respect to the chosen time-step interval in the environment. + + """ + + _env: ManagerBasedRLEnv + """The environment instance.""" + + def __init__(self, cfg: object, env: ManagerBasedRLEnv): + """Initialize the reward manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, RewardTermCfg]``). + env: The environment instance. + """ + # create buffers to parse and store terms + self._term_names: list[str] = list() + self._term_cfgs: list[RewardTermCfg] = list() + self._class_term_cfgs: list[RewardTermCfg] = list() + + # call the base class constructor (this will parse the terms config) + super().__init__(cfg, env) + # prepare extra info to store individual reward term information + self._episode_sums = dict() + for term_name in self._term_names: + self._episode_sums[term_name] = torch.zeros(self.num_envs, dtype=torch.float, device=self.device) + # create buffer for managing reward per environment + self._reward_buf = torch.zeros(self.num_envs, dtype=torch.float, device=self.device) + + # Buffer which stores the current step reward for each term for each environment + self._step_reward = torch.zeros((self.num_envs, len(self._term_names)), dtype=torch.float, device=self.device) + + def __str__(self) -> str: + """Returns: A string representation for reward manager.""" + msg = f" contains {len(self._term_names)} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = "Active Reward Terms" + table.field_names = ["Index", "Name", "Weight"] + # set alignment of table columns + table.align["Name"] = "l" + table.align["Weight"] = "r" + # add info on each term + for index, (name, term_cfg) in enumerate(zip(self._term_names, self._term_cfgs)): + table.add_row([index, name, term_cfg.weight]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def active_terms(self) -> list[str]: + """Name of active reward terms.""" + return self._term_names + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Returns the episodic sum of individual reward terms. + + Args: + env_ids: The environment ids for which the episodic sum of + individual reward terms is to be returned. Defaults to all the environment ids. + + Returns: + Dictionary of episodic sum of individual reward terms. + """ + # resolve environment ids + if env_ids is None: + env_ids = slice(None) + # store information + extras = {} + for key in self._episode_sums.keys(): + # store information + # r_1 + r_2 + ... + r_n + episodic_sum_avg = torch.mean(self._episode_sums[key][env_ids]) + extras["Episode_Reward/" + key] = episodic_sum_avg / self._env.max_episode_length_s + # reset episodic sum + self._episode_sums[key][env_ids] = 0.0 + # reset all the reward terms + for term_cfg in self._class_term_cfgs: + term_cfg.func.reset(env_ids=env_ids) + # return logged information + return extras + + def compute(self, dt: float) -> torch.Tensor: + """Computes the reward signal as a weighted sum of individual terms. + + This function calls each reward term managed by the class and adds them to compute the net + reward signal. It also updates the episodic sums corresponding to individual reward terms. + + Args: + dt: The time-step interval of the environment. + + Returns: + The net reward signal of shape (num_envs,). + """ + # reset computation + self._reward_buf[:] = 0.0 + # iterate over all the reward terms + for term_idx, (name, term_cfg) in enumerate(zip(self._term_names, self._term_cfgs)): + # skip if weight is zero (kind of a micro-optimization) + if term_cfg.weight == 0.0: + self._step_reward[:, term_idx] = 0.0 + continue + # compute term's value + value = term_cfg.func(self._env, **term_cfg.params) * term_cfg.weight * dt + # update total reward + self._reward_buf += value + # update episodic sum + self._episode_sums[name] += value + + # Update current reward for this step. + self._step_reward[:, term_idx] = value / dt + + return self._reward_buf + + """ + Operations - Term settings. + """ + + def set_term_cfg(self, term_name: str, cfg: RewardTermCfg): + """Sets the configuration of the specified term into the manager. + + Args: + term_name: The name of the reward term. + cfg: The configuration for the reward term. + + Raises: + ValueError: If the term name is not found. + """ + if term_name not in self._term_names: + raise ValueError(f"Reward term '{term_name}' not found.") + # set the configuration + self._term_cfgs[self._term_names.index(term_name)] = cfg + + def get_term_cfg(self, term_name: str) -> RewardTermCfg: + """Gets the configuration for the specified term. + + Args: + term_name: The name of the reward term. + + Returns: + The configuration of the reward term. + + Raises: + ValueError: If the term name is not found. + """ + if term_name not in self._term_names: + raise ValueError(f"Reward term '{term_name}' not found.") + # return the configuration + return self._term_cfgs[self._term_names.index(term_name)] + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + terms = [] + for idx, name in enumerate(self._term_names): + terms.append((name, [self._step_reward[env_idx, idx].cpu().item()])) + return terms + + """ + Helper functions. + """ + + def _prepare_terms(self): + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # check for valid config type + if not isinstance(term_cfg, RewardTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type RewardTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # check for valid weight type + if not isinstance(term_cfg.weight, (float, int)): + raise TypeError( + f"Weight for the term '{term_name}' is not of type float or int." + f" Received: '{type(term_cfg.weight)}'." + ) + # resolve common parameters + self._resolve_common_term_cfg(term_name, term_cfg, min_argc=1) + # add function to list + self._term_names.append(term_name) + self._term_cfgs.append(term_cfg) + # check if the term is a class + if isinstance(term_cfg.func, ManagerTermBase): + self._class_term_cfgs.append(term_cfg) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/scene_entity_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/scene_entity_cfg.py new file mode 100644 index 00000000000..64a05cbc0b4 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/scene_entity_cfg.py @@ -0,0 +1,290 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration terms for different managers.""" + +from dataclasses import MISSING + +from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection +from isaaclab.scene import InteractiveScene +from isaaclab.utils import configclass + + +@configclass +class SceneEntityCfg: + """Configuration for a scene entity that is used by the manager's term. + + This class is used to specify the name of the scene entity that is queried from the + :class:`InteractiveScene` and passed to the manager's term function. + """ + + name: str = MISSING + """The name of the scene entity. + + This is the name defined in the scene configuration file. See the :class:`InteractiveSceneCfg` + class for more details. + """ + + joint_names: str | list[str] | None = None + """The names of the joints from the scene entity. Defaults to None. + + The names can be either joint names or a regular expression matching the joint names. + + These are converted to joint indices on initialization of the manager and passed to the term + function as a list of joint indices under :attr:`joint_ids`. + """ + + joint_ids: list[int] | slice = slice(None) + """The indices of the joints from the asset required by the term. Defaults to slice(None), which means + all the joints in the asset (if present). + + If :attr:`joint_names` is specified, this is filled in automatically on initialization of the + manager. + """ + + fixed_tendon_names: str | list[str] | None = None + """The names of the fixed tendons from the scene entity. Defaults to None. + + The names can be either joint names or a regular expression matching the joint names. + + These are converted to fixed tendon indices on initialization of the manager and passed to the term + function as a list of fixed tendon indices under :attr:`fixed_tendon_ids`. + """ + + fixed_tendon_ids: list[int] | slice = slice(None) + """The indices of the fixed tendons from the asset required by the term. Defaults to slice(None), which means + all the fixed tendons in the asset (if present). + + If :attr:`fixed_tendon_names` is specified, this is filled in automatically on initialization of the + manager. + """ + + body_names: str | list[str] | None = None + """The names of the bodies from the asset required by the term. Defaults to None. + + The names can be either body names or a regular expression matching the body names. + + These are converted to body indices on initialization of the manager and passed to the term + function as a list of body indices under :attr:`body_ids`. + """ + + body_ids: list[int] | slice = slice(None) + """The indices of the bodies from the asset required by the term. Defaults to slice(None), which means + all the bodies in the asset. + + If :attr:`body_names` is specified, this is filled in automatically on initialization of the + manager. + """ + + object_collection_names: str | list[str] | None = None + """The names of the objects in the rigid object collection required by the term. Defaults to None. + + The names can be either names or a regular expression matching the object names in the collection. + + These are converted to object indices on initialization of the manager and passed to the term + function as a list of object indices under :attr:`object_collection_ids`. + """ + + object_collection_ids: list[int] | slice = slice(None) + """The indices of the objects from the rigid object collection required by the term. Defaults to slice(None), + which means all the objects in the collection. + + If :attr:`object_collection_names` is specified, this is filled in automatically on initialization of the manager. + """ + + preserve_order: bool = False + """Whether to preserve indices ordering to match with that in the specified joint, body, or object collection names. + Defaults to False. + + If False, the ordering of the indices are sorted in ascending order (i.e. the ordering in the entity's joints, + bodies, or object in the object collection). Otherwise, the indices are preserved in the order of the specified + joint, body, or object collection names. + + For more details, see the :meth:`isaaclab.utils.string.resolve_matching_names` function. + + .. note:: + This attribute is only used when :attr:`joint_names`, :attr:`body_names`, or :attr:`object_collection_names` are specified. + + """ + + def resolve(self, scene: InteractiveScene): + """Resolves the scene entity and converts the joint and body names to indices. + + This function examines the scene entity from the :class:`InteractiveScene` and resolves the indices + and names of the joints and bodies. It is an expensive operation as it resolves regular expressions + and should be called only once. + + Args: + scene: The interactive scene instance. + + Raises: + ValueError: If the scene entity is not found. + ValueError: If both ``joint_names`` and ``joint_ids`` are specified and are not consistent. + ValueError: If both ``fixed_tendon_names`` and ``fixed_tendon_ids`` are specified and are not consistent. + ValueError: If both ``body_names`` and ``body_ids`` are specified and are not consistent. + ValueError: If both ``object_collection_names`` and ``object_collection_ids`` are specified and are not consistent. + """ + # check if the entity is valid + if self.name not in scene.keys(): + raise ValueError(f"The scene entity '{self.name}' does not exist. Available entities: {scene.keys()}.") + + # convert joint names to indices based on regex + self._resolve_joint_names(scene) + + # convert fixed tendon names to indices based on regex + self._resolve_fixed_tendon_names(scene) + + # convert body names to indices based on regex + self._resolve_body_names(scene) + + # convert object collection names to indices based on regex + self._resolve_object_collection_names(scene) + + def _resolve_joint_names(self, scene: InteractiveScene): + # convert joint names to indices based on regex + if self.joint_names is not None or self.joint_ids != slice(None): + entity: Articulation = scene[self.name] + # -- if both are not their default values, check if they are valid + if self.joint_names is not None and self.joint_ids != slice(None): + if isinstance(self.joint_names, str): + self.joint_names = [self.joint_names] + if isinstance(self.joint_ids, int): + self.joint_ids = [self.joint_ids] + joint_ids, _ = entity.find_joints(self.joint_names, preserve_order=self.preserve_order) + joint_names = [entity.joint_names[i] for i in self.joint_ids] + if joint_ids != self.joint_ids or joint_names != self.joint_names: + raise ValueError( + "Both 'joint_names' and 'joint_ids' are specified, and are not consistent." + f"\n\tfrom joint names: {self.joint_names} [{joint_ids}]" + f"\n\tfrom joint ids: {joint_names} [{self.joint_ids}]" + "\nHint: Use either 'joint_names' or 'joint_ids' to avoid confusion." + ) + # -- from joint names to joint indices + elif self.joint_names is not None: + if isinstance(self.joint_names, str): + self.joint_names = [self.joint_names] + self.joint_ids, _ = entity.find_joints(self.joint_names, preserve_order=self.preserve_order) + # performance optimization (slice offers faster indexing than list of indices) + # only all joint in the entity order are selected + if len(self.joint_ids) == entity.num_joints and self.joint_names == entity.joint_names: + self.joint_ids = slice(None) + # -- from joint indices to joint names + elif self.joint_ids != slice(None): + if isinstance(self.joint_ids, int): + self.joint_ids = [self.joint_ids] + self.joint_names = [entity.joint_names[i] for i in self.joint_ids] + + def _resolve_fixed_tendon_names(self, scene: InteractiveScene): + # convert tendon names to indices based on regex + if self.fixed_tendon_names is not None or self.fixed_tendon_ids != slice(None): + entity: Articulation = scene[self.name] + # -- if both are not their default values, check if they are valid + if self.fixed_tendon_names is not None and self.fixed_tendon_ids != slice(None): + if isinstance(self.fixed_tendon_names, str): + self.fixed_tendon_names = [self.fixed_tendon_names] + if isinstance(self.fixed_tendon_ids, int): + self.fixed_tendon_ids = [self.fixed_tendon_ids] + fixed_tendon_ids, _ = entity.find_fixed_tendons( + self.fixed_tendon_names, preserve_order=self.preserve_order + ) + fixed_tendon_names = [entity.fixed_tendon_names[i] for i in self.fixed_tendon_ids] + if fixed_tendon_ids != self.fixed_tendon_ids or fixed_tendon_names != self.fixed_tendon_names: + raise ValueError( + "Both 'fixed_tendon_names' and 'fixed_tendon_ids' are specified, and are not consistent." + f"\n\tfrom joint names: {self.fixed_tendon_names} [{fixed_tendon_ids}]" + f"\n\tfrom joint ids: {fixed_tendon_names} [{self.fixed_tendon_ids}]" + "\nHint: Use either 'fixed_tendon_names' or 'fixed_tendon_ids' to avoid confusion." + ) + # -- from fixed tendon names to fixed tendon indices + elif self.fixed_tendon_names is not None: + if isinstance(self.fixed_tendon_names, str): + self.fixed_tendon_names = [self.fixed_tendon_names] + self.fixed_tendon_ids, _ = entity.find_fixed_tendons( + self.fixed_tendon_names, preserve_order=self.preserve_order + ) + # performance optimization (slice offers faster indexing than list of indices) + # only all fixed tendon in the entity order are selected + if ( + len(self.fixed_tendon_ids) == entity.num_fixed_tendons + and self.fixed_tendon_names == entity.fixed_tendon_names + ): + self.fixed_tendon_ids = slice(None) + # -- from fixed tendon indices to fixed tendon names + elif self.fixed_tendon_ids != slice(None): + if isinstance(self.fixed_tendon_ids, int): + self.fixed_tendon_ids = [self.fixed_tendon_ids] + self.fixed_tendon_names = [entity.fixed_tendon_names[i] for i in self.fixed_tendon_ids] + + def _resolve_body_names(self, scene: InteractiveScene): + # convert body names to indices based on regex + if self.body_names is not None or self.body_ids != slice(None): + entity: RigidObject = scene[self.name] + # -- if both are not their default values, check if they are valid + if self.body_names is not None and self.body_ids != slice(None): + if isinstance(self.body_names, str): + self.body_names = [self.body_names] + if isinstance(self.body_ids, int): + self.body_ids = [self.body_ids] + body_ids, _ = entity.find_bodies(self.body_names, preserve_order=self.preserve_order) + body_names = [entity.body_names[i] for i in self.body_ids] + if body_ids != self.body_ids or body_names != self.body_names: + raise ValueError( + "Both 'body_names' and 'body_ids' are specified, and are not consistent." + f"\n\tfrom body names: {self.body_names} [{body_ids}]" + f"\n\tfrom body ids: {body_names} [{self.body_ids}]" + "\nHint: Use either 'body_names' or 'body_ids' to avoid confusion." + ) + # -- from body names to body indices + elif self.body_names is not None: + if isinstance(self.body_names, str): + self.body_names = [self.body_names] + self.body_ids, _ = entity.find_bodies(self.body_names, preserve_order=self.preserve_order) + # performance optimization (slice offers faster indexing than list of indices) + # only all bodies in the entity order are selected + if len(self.body_ids) == entity.num_bodies and self.body_names == entity.body_names: + self.body_ids = slice(None) + # -- from body indices to body names + elif self.body_ids != slice(None): + if isinstance(self.body_ids, int): + self.body_ids = [self.body_ids] + self.body_names = [entity.body_names[i] for i in self.body_ids] + + def _resolve_object_collection_names(self, scene: InteractiveScene): + # convert object names to indices based on regex + if self.object_collection_names is not None or self.object_collection_ids != slice(None): + entity: RigidObjectCollection = scene[self.name] + # -- if both are not their default values, check if they are valid + if self.object_collection_names is not None and self.object_collection_ids != slice(None): + if isinstance(self.object_collection_names, str): + self.object_collection_names = [self.object_collection_names] + if isinstance(self.object_collection_ids, int): + self.object_collection_ids = [self.object_collection_ids] + object_ids, _ = entity.find_objects(self.object_collection_names, preserve_order=self.preserve_order) + object_names = [entity.object_names[i] for i in self.object_collection_ids] + if object_ids != self.object_collection_ids or object_names != self.object_collection_names: + raise ValueError( + "Both 'object_collection_names' and 'object_collection_ids' are specified, and are not" + " consistent.\n\tfrom object collection names:" + f" {self.object_collection_names} [{object_ids}]\n\tfrom object collection ids:" + f" {object_names} [{self.object_collection_ids}]\nHint: Use either 'object_collection_names' or" + " 'object_collection_ids' to avoid confusion." + ) + # -- from object names to object indices + elif self.object_collection_names is not None: + if isinstance(self.object_collection_names, str): + self.object_collection_names = [self.object_collection_names] + self.object_collection_ids, _ = entity.find_objects( + self.object_collection_names, preserve_order=self.preserve_order + ) + # -- from object indices to object names + elif self.object_collection_ids != slice(None): + if isinstance(self.object_collection_ids, int): + self.object_collection_ids = [self.object_collection_ids] + self.object_collection_names = [entity.object_names[i] for i in self.object_collection_ids] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/termination_manager.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/termination_manager.py new file mode 100644 index 00000000000..4c7052bb14e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/managers/termination_manager.py @@ -0,0 +1,272 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Termination manager for computing done signals for a given world.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from prettytable import PrettyTable +from typing import TYPE_CHECKING + +from .manager_base import ManagerBase, ManagerTermBase +from .manager_term_cfg import TerminationTermCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class TerminationManager(ManagerBase): + """Manager for computing done signals for a given world. + + The termination manager computes the termination signal (also called dones) as a combination + of termination terms. Each termination term is a function which takes the environment as an + argument and returns a boolean tensor of shape (num_envs,). The termination manager + computes the termination signal as the union (logical or) of all the termination terms. + + Following the `Gymnasium API `_, + the termination signal is computed as the logical OR of the following signals: + + * **Time-out**: This signal is set to true if the environment has ended after an externally defined condition + (that is outside the scope of a MDP). For example, the environment may be terminated if the episode has + timed out (i.e. reached max episode length). + * **Terminated**: This signal is set to true if the environment has reached a terminal state defined by the + environment. This state may correspond to task success, task failure, robot falling, etc. + + These signals can be individually accessed using the :attr:`time_outs` and :attr:`terminated` properties. + + The termination terms are parsed from a config class containing the manager's settings and each term's + parameters. Each termination term should instantiate the :class:`TerminationTermCfg` class. The term's + configuration :attr:`TerminationTermCfg.time_out` decides whether the term is a timeout or a termination term. + """ + + _env: ManagerBasedRLEnv + """The environment instance.""" + + def __init__(self, cfg: object, env: ManagerBasedRLEnv): + """Initializes the termination manager. + + Args: + cfg: The configuration object or dictionary (``dict[str, TerminationTermCfg]``). + env: An environment object. + """ + # create buffers to parse and store terms + self._term_names: list[str] = list() + self._term_cfgs: list[TerminationTermCfg] = list() + self._class_term_cfgs: list[TerminationTermCfg] = list() + + # call the base class constructor (this will parse the terms config) + super().__init__(cfg, env) + # prepare extra info to store individual termination term information + self._term_dones = dict() + for term_name in self._term_names: + self._term_dones[term_name] = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + # create buffer for managing termination per environment + self._truncated_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self._terminated_buf = torch.zeros_like(self._truncated_buf) + + def __str__(self) -> str: + """Returns: A string representation for termination manager.""" + msg = f" contains {len(self._term_names)} active terms.\n" + + # create table for term information + table = PrettyTable() + table.title = "Active Termination Terms" + table.field_names = ["Index", "Name", "Time Out"] + # set alignment of table columns + table.align["Name"] = "l" + # add info on each term + for index, (name, term_cfg) in enumerate(zip(self._term_names, self._term_cfgs)): + table.add_row([index, name, term_cfg.time_out]) + # convert table to string + msg += table.get_string() + msg += "\n" + + return msg + + """ + Properties. + """ + + @property + def active_terms(self) -> list[str]: + """Name of active termination terms.""" + return self._term_names + + @property + def dones(self) -> torch.Tensor: + """The net termination signal. Shape is (num_envs,).""" + return self._truncated_buf | self._terminated_buf + + @property + def time_outs(self) -> torch.Tensor: + """The timeout signal (reaching max episode length). Shape is (num_envs,). + + This signal is set to true if the environment has ended after an externally defined condition + (that is outside the scope of a MDP). For example, the environment may be terminated if the episode has + timed out (i.e. reached max episode length). + """ + return self._truncated_buf + + @property + def terminated(self) -> torch.Tensor: + """The terminated signal (reaching a terminal state). Shape is (num_envs,). + + This signal is set to true if the environment has reached a terminal state defined by the environment. + This state may correspond to task success, task failure, robot falling, etc. + """ + return self._terminated_buf + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Returns the episodic counts of individual termination terms. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + + Returns: + Dictionary of episodic sum of individual reward terms. + """ + # resolve environment ids + if env_ids is None: + env_ids = slice(None) + # add to episode dict + extras = {} + for key in self._term_dones.keys(): + # store information + extras["Episode_Termination/" + key] = torch.count_nonzero(self._term_dones[key][env_ids]).item() + # reset all the reward terms + for term_cfg in self._class_term_cfgs: + term_cfg.func.reset(env_ids=env_ids) + # return logged information + return extras + + def compute(self) -> torch.Tensor: + """Computes the termination signal as union of individual terms. + + This function calls each termination term managed by the class and performs a logical OR operation + to compute the net termination signal. + + Returns: + The combined termination signal of shape (num_envs,). + """ + # reset computation + self._truncated_buf[:] = False + self._terminated_buf[:] = False + # iterate over all the termination terms + for name, term_cfg in zip(self._term_names, self._term_cfgs): + value = term_cfg.func(self._env, **term_cfg.params) + # store timeout signal separately + if term_cfg.time_out: + self._truncated_buf |= value + else: + self._terminated_buf |= value + # add to episode dones + self._term_dones[name][:] = value + # return combined termination signal + return self._truncated_buf | self._terminated_buf + + def get_term(self, name: str) -> torch.Tensor: + """Returns the termination term with the specified name. + + Args: + name: The name of the termination term. + + Returns: + The corresponding termination term value. Shape is (num_envs,). + """ + return self._term_dones[name] + + def get_active_iterable_terms(self, env_idx: int) -> Sequence[tuple[str, Sequence[float]]]: + """Returns the active terms as iterable sequence of tuples. + + The first element of the tuple is the name of the term and the second element is the raw value(s) of the term. + + Args: + env_idx: The specific environment to pull the active terms from. + + Returns: + The active terms. + """ + terms = [] + for key in self._term_dones.keys(): + terms.append((key, [self._term_dones[key][env_idx].float().cpu().item()])) + return terms + + """ + Operations - Term settings. + """ + + def set_term_cfg(self, term_name: str, cfg: TerminationTermCfg): + """Sets the configuration of the specified term into the manager. + + Args: + term_name: The name of the termination term. + cfg: The configuration for the termination term. + + Raises: + ValueError: If the term name is not found. + """ + if term_name not in self._term_names: + raise ValueError(f"Termination term '{term_name}' not found.") + # set the configuration + self._term_cfgs[self._term_names.index(term_name)] = cfg + + def get_term_cfg(self, term_name: str) -> TerminationTermCfg: + """Gets the configuration for the specified term. + + Args: + term_name: The name of the termination term. + + Returns: + The configuration of the termination term. + + Raises: + ValueError: If the term name is not found. + """ + if term_name not in self._term_names: + raise ValueError(f"Termination term '{term_name}' not found.") + # return the configuration + return self._term_cfgs[self._term_names.index(term_name)] + + """ + Helper functions. + """ + + def _prepare_terms(self): + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + # iterate over all the terms + for term_name, term_cfg in cfg_items: + # check for non config + if term_cfg is None: + continue + # check for valid config type + if not isinstance(term_cfg, TerminationTermCfg): + raise TypeError( + f"Configuration for the term '{term_name}' is not of type TerminationTermCfg." + f" Received: '{type(term_cfg)}'." + ) + # resolve common parameters + self._resolve_common_term_cfg(term_name, term_cfg, min_argc=1) + # add function to list + self._term_names.append(term_name) + self._term_cfgs.append(term_cfg) + # check if the term is a class + if isinstance(term_cfg.func, ManagerTermBase): + self._class_term_cfgs.append(term_cfg) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/__init__.py new file mode 100644 index 00000000000..df610863913 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/__init__.py @@ -0,0 +1,30 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for marker utilities to simplify creation of UI elements in the GUI. + +Currently, the sub-package provides the following classes: + +* :class:`VisualizationMarkers` for creating a group of markers using `UsdGeom.PointInstancer + `_. + + +.. note:: + + For some simple use-cases, it may be sufficient to use the debug drawing utilities from Isaac Sim. + The debug drawing API is available in the `isaacsim.util.debug_drawing`_ module. It allows drawing of + points and splines efficiently on the UI. + + .. _isaacsim.util.debug_drawing: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_debug_drawing.html + +""" + +from .config import * # noqa: F401, F403 +from .visualization_markers import VisualizationMarkers, VisualizationMarkersCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/config/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/config/__init__.py new file mode 100644 index 00000000000..1415c65e8d5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/config/__init__.py @@ -0,0 +1,137 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.markers.visualization_markers import VisualizationMarkersCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Sensors. +## + +RAY_CASTER_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "hit": sim_utils.SphereCfg( + radius=0.02, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, +) +"""Configuration for the ray-caster marker.""" + + +CONTACT_SENSOR_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "contact": sim_utils.SphereCfg( + radius=0.02, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + "no_contact": sim_utils.SphereCfg( + radius=0.02, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + visible=False, + ), + }, +) +"""Configuration for the contact sensor marker.""" + +DEFORMABLE_TARGET_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "target": sim_utils.SphereCfg( + radius=0.02, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.75, 0.8)), + ), + }, +) +"""Configuration for the deformable object's kinematic target marker.""" + + +## +# Frames. +## + +FRAME_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "frame": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.5, 0.5, 0.5), + ) + } +) +"""Configuration for the frame marker.""" + + +RED_ARROW_X_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "arrow": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd", + scale=(1.0, 0.1, 0.1), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ) + } +) +"""Configuration for the red arrow marker (along x-direction).""" + + +BLUE_ARROW_X_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "arrow": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd", + scale=(1.0, 0.1, 0.1), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ) + } +) +"""Configuration for the blue arrow marker (along x-direction).""" + +GREEN_ARROW_X_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "arrow": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd", + scale=(1.0, 0.1, 0.1), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ) + } +) +"""Configuration for the green arrow marker (along x-direction).""" + + +## +# Goals. +## + +CUBOID_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "cuboid": sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + } +) +"""Configuration for the cuboid marker.""" + +POSITION_GOAL_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "target_far": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + "target_near": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + "target_invisible": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + visible=False, + ), + } +) +"""Configuration for the end-effector tracking marker.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/visualization_markers.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/visualization_markers.py new file mode 100644 index 00000000000..045c8a49a09 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/markers/visualization_markers.py @@ -0,0 +1,415 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A class to coordinate groups of visual markers (such as spheres, frames or arrows) +using `UsdGeom.PointInstancer`_ class. + +The class :class:`VisualizationMarkers` is used to create a group of visual markers and +visualize them in the viewport. The markers are represented as :class:`UsdGeom.PointInstancer` prims +in the USD stage. The markers are created as prototypes in the :class:`UsdGeom.PointInstancer` prim +and are instanced in the :class:`UsdGeom.PointInstancer` prim. The markers can be visualized by +passing the indices of the marker prototypes and their translations, orientations and scales. +The marker prototypes can be configured with the :class:`VisualizationMarkersCfg` class. + +.. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html +""" + +# needed to import for allowing type-hinting: np.ndarray | torch.Tensor | None +from __future__ import annotations + +import numpy as np +import torch +from dataclasses import MISSING + +import isaacsim.core.utils.stage as stage_utils +import omni.kit.commands +import omni.physx.scripts.utils as physx_utils +from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, Vt + +import isaaclab.sim as sim_utils +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.utils.configclass import configclass +from isaaclab.utils.math import convert_quat + + +@configclass +class VisualizationMarkersCfg: + """A class to configure a :class:`VisualizationMarkers`.""" + + prim_path: str = MISSING + """The prim path where the :class:`UsdGeom.PointInstancer` will be created.""" + + markers: dict[str, SpawnerCfg] = MISSING + """The dictionary of marker configurations. + + The key is the name of the marker, and the value is the configuration of the marker. + The key is used to identify the marker in the class. + """ + + +class VisualizationMarkers: + """A class to coordinate groups of visual markers (loaded from USD). + + This class allows visualization of different UI markers in the scene, such as points and frames. + The class wraps around the `UsdGeom.PointInstancer`_ for efficient handling of objects + in the stage via instancing the created marker prototype prims. + + A marker prototype prim is a reusable template prim used for defining variations of objects + in the scene. For example, a sphere prim can be used as a marker prototype prim to create + multiple sphere prims in the scene at different locations. Thus, prototype prims are useful + for creating multiple instances of the same prim in the scene. + + The class parses the configuration to create different the marker prototypes into the stage. Each marker + prototype prim is created as a child of the :class:`UsdGeom.PointInstancer` prim. The prim path for the + the marker prim is resolved using the key of the marker in the :attr:`VisualizationMarkersCfg.markers` + dictionary. The marker prototypes are created using the :meth:`isaacsim.core.utils.create_prim` + function, and then then instanced using :class:`UsdGeom.PointInstancer` prim to allow creating multiple + instances of the marker prims. + + Switching between different marker prototypes is possible by calling the :meth:`visualize` method with + the prototype indices corresponding to the marker prototype. The prototype indices are based on the order + in the :attr:`VisualizationMarkersCfg.markers` dictionary. For example, if the dictionary has two markers, + "marker1" and "marker2", then their prototype indices are 0 and 1 respectively. The prototype indices + can be passed as a list or array of integers. + + Usage: + The following snippet shows how to create 24 sphere markers with a radius of 1.0 at random translations + within the range [-1.0, 1.0]. The first 12 markers will be colored red and the rest will be colored green. + + .. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab.markers import VisualizationMarkersCfg, VisualizationMarkers + + # Create the markers configuration + # This creates two marker prototypes, "marker1" and "marker2" which are spheres with a radius of 1.0. + # The color of "marker1" is red and the color of "marker2" is green. + cfg = VisualizationMarkersCfg( + prim_path="/World/Visuals/testMarkers", + markers={ + "marker1": sim_utils.SphereCfg( + radius=1.0, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + "marker2": VisualizationMarkersCfg.SphereCfg( + radius=1.0, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + } + ) + # Create the markers instance + # This will create a UsdGeom.PointInstancer prim at the given path along with the marker prototypes. + marker = VisualizationMarkers(cfg) + + # Set position of the marker + # -- randomly sample translations between -1.0 and 1.0 + marker_translations = np.random.uniform(-1.0, 1.0, (24, 3)) + # -- this will create 24 markers at the given translations + # note: the markers will all be `marker1` since the marker indices are not given + marker.visualize(translations=marker_translations) + + # alter the markers based on their prototypes indices + # first 12 markers will be marker1 and the rest will be marker2 + # 0 -> marker1, 1 -> marker2 + marker_indices = [0] * 12 + [1] * 12 + # this will change the marker prototypes at the given indices + # note: the translations of the markers will not be changed from the previous call + # since the translations are not given. + marker.visualize(marker_indices=marker_indices) + + # alter the markers based on their prototypes indices and translations + marker.visualize(marker_indices=marker_indices, translations=marker_translations) + + .. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html + + """ + + def __init__(self, cfg: VisualizationMarkersCfg): + """Initialize the class. + + When the class is initialized, the :class:`UsdGeom.PointInstancer` is created into the stage + and the marker prims are registered into it. + + .. note:: + If a prim already exists at the given path, the function will find the next free path + and create the :class:`UsdGeom.PointInstancer` prim there. + + Args: + cfg: The configuration for the markers. + + Raises: + ValueError: When no markers are provided in the :obj:`cfg`. + """ + # get next free path for the prim + prim_path = stage_utils.get_next_free_path(cfg.prim_path) + # create a new prim + stage = stage_utils.get_current_stage() + self._instancer_manager = UsdGeom.PointInstancer.Define(stage, prim_path) + # store inputs + self.prim_path = prim_path + self.cfg = cfg + # check if any markers is provided + if len(self.cfg.markers) == 0: + raise ValueError(f"The `cfg.markers` cannot be empty. Received: {self.cfg.markers}") + + # create a child prim for the marker + self._add_markers_prototypes(self.cfg.markers) + # Note: We need to do this the first time to initialize the instancer. + # Otherwise, the instancer will not be "created" and the function `GetInstanceIndices()` will fail. + self._instancer_manager.GetProtoIndicesAttr().Set(list(range(self.num_prototypes))) + self._instancer_manager.GetPositionsAttr().Set([Gf.Vec3f(0.0)] * self.num_prototypes) + self._count = self.num_prototypes + + def __str__(self) -> str: + """Return: A string representation of the class.""" + msg = f"VisualizationMarkers(prim_path={self.prim_path})" + msg += f"\n\tCount: {self.count}" + msg += f"\n\tNumber of prototypes: {self.num_prototypes}" + msg += "\n\tMarkers Prototypes:" + for index, (name, marker) in enumerate(self.cfg.markers.items()): + msg += f"\n\t\t[Index: {index}]: {name}: {marker.to_dict()}" + return msg + + """ + Properties. + """ + + @property + def num_prototypes(self) -> int: + """The number of marker prototypes available.""" + return len(self.cfg.markers) + + @property + def count(self) -> int: + """The total number of marker instances.""" + # TODO: Update this when the USD API is available (Isaac Sim 2023.1) + # return self._instancer_manager.GetInstanceCount() + return self._count + + """ + Operations. + """ + + def set_visibility(self, visible: bool): + """Sets the visibility of the markers. + + The method does this through the USD API. + + Args: + visible: flag to set the visibility. + """ + imageable = UsdGeom.Imageable(self._instancer_manager) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def is_visible(self) -> bool: + """Checks the visibility of the markers. + + Returns: + True if the markers are visible, False otherwise. + """ + return self._instancer_manager.GetVisibilityAttr().Get() != UsdGeom.Tokens.invisible + + def visualize( + self, + translations: np.ndarray | torch.Tensor | None = None, + orientations: np.ndarray | torch.Tensor | None = None, + scales: np.ndarray | torch.Tensor | None = None, + marker_indices: list[int] | np.ndarray | torch.Tensor | None = None, + ): + """Update markers in the viewport. + + .. note:: + If the prim `PointInstancer` is hidden in the stage, the function will simply return + without updating the markers. This helps in unnecessary computation when the markers + are not visible. + + Whenever updating the markers, the input arrays must have the same number of elements + in the first dimension. If the number of elements is different, the `UsdGeom.PointInstancer` + will raise an error complaining about the mismatch. + + Additionally, the function supports dynamic update of the markers. This means that the + number of markers can change between calls. For example, if you have 24 points that you + want to visualize, you can pass 24 translations, orientations, and scales. If you want to + visualize only 12 points, you can pass 12 translations, orientations, and scales. The + function will automatically update the number of markers in the scene. + + The function will also update the marker prototypes based on their prototype indices. For instance, + if you have two marker prototypes, and you pass the following marker indices: [0, 1, 0, 1], the function + will update the first and third markers with the first prototype, and the second and fourth markers + with the second prototype. This is useful when you want to visualize different markers in the same + scene. The list of marker indices must have the same number of elements as the translations, orientations, + or scales. If the number of elements is different, the function will raise an error. + + .. caution:: + This function will update all the markers instanced from the prototypes. That means + if you have 24 markers, you will need to pass 24 translations, orientations, and scales. + + If you want to update only a subset of the markers, you will need to handle the indices + yourself and pass the complete arrays to this function. + + Args: + translations: Translations w.r.t. parent prim frame. Shape is (M, 3). + Defaults to None, which means left unchanged. + orientations: Quaternion orientations (w, x, y, z) w.r.t. parent prim frame. Shape is (M, 4). + Defaults to None, which means left unchanged. + scales: Scale applied before any rotation is applied. Shape is (M, 3). + Defaults to None, which means left unchanged. + marker_indices: Decides which marker prototype to visualize. Shape is (M). + Defaults to None, which means left unchanged provided that the total number of markers + is the same as the previous call. If the number of markers is different, the function + will update the number of markers in the scene. + + Raises: + ValueError: When input arrays do not follow the expected shapes. + ValueError: When the function is called with all None arguments. + """ + # check if it is visible (if not then let's not waste time) + if not self.is_visible(): + return + # check if we have any markers to visualize + num_markers = 0 + # resolve inputs + # -- position + if translations is not None: + if isinstance(translations, torch.Tensor): + translations = translations.detach().cpu().numpy() + # check that shape is correct + if translations.shape[1] != 3 or len(translations.shape) != 2: + raise ValueError(f"Expected `translations` to have shape (M, 3). Received: {translations.shape}.") + # apply translations + self._instancer_manager.GetPositionsAttr().Set(Vt.Vec3fArray.FromNumpy(translations)) + # update number of markers + num_markers = translations.shape[0] + # -- orientation + if orientations is not None: + if isinstance(orientations, torch.Tensor): + orientations = orientations.detach().cpu().numpy() + # check that shape is correct + if orientations.shape[1] != 4 or len(orientations.shape) != 2: + raise ValueError(f"Expected `orientations` to have shape (M, 4). Received: {orientations.shape}.") + # roll orientations from (w, x, y, z) to (x, y, z, w) + # internally USD expects (x, y, z, w) + orientations = convert_quat(orientations, to="xyzw") + # apply orientations + self._instancer_manager.GetOrientationsAttr().Set(Vt.QuathArray.FromNumpy(orientations)) + # update number of markers + num_markers = orientations.shape[0] + # -- scales + if scales is not None: + if isinstance(scales, torch.Tensor): + scales = scales.detach().cpu().numpy() + # check that shape is correct + if scales.shape[1] != 3 or len(scales.shape) != 2: + raise ValueError(f"Expected `scales` to have shape (M, 3). Received: {scales.shape}.") + # apply scales + self._instancer_manager.GetScalesAttr().Set(Vt.Vec3fArray.FromNumpy(scales)) + # update number of markers + num_markers = scales.shape[0] + # -- status + if marker_indices is not None or num_markers != self._count: + # apply marker indices + if marker_indices is not None: + if isinstance(marker_indices, torch.Tensor): + marker_indices = marker_indices.detach().cpu().numpy() + elif isinstance(marker_indices, list): + marker_indices = np.array(marker_indices) + # check that shape is correct + if len(marker_indices.shape) != 1: + raise ValueError(f"Expected `marker_indices` to have shape (M,). Received: {marker_indices.shape}.") + # apply proto indices + self._instancer_manager.GetProtoIndicesAttr().Set(Vt.IntArray.FromNumpy(marker_indices)) + # update number of markers + num_markers = marker_indices.shape[0] + else: + # check that number of markers is not zero + if num_markers == 0: + raise ValueError("Number of markers cannot be zero! Hint: The function was called with no inputs?") + # set all markers to be the first prototype + self._instancer_manager.GetProtoIndicesAttr().Set([0] * num_markers) + # set number of markers + self._count = num_markers + + """ + Helper functions. + """ + + def _add_markers_prototypes(self, markers_cfg: dict[str, sim_utils.SpawnerCfg]): + """Adds markers prototypes to the scene and sets the markers instancer to use them.""" + # add markers based on config + for name, cfg in markers_cfg.items(): + # resolve prim path + marker_prim_path = f"{self.prim_path}/{name}" + # create a child prim for the marker + marker_prim = cfg.func(prim_path=marker_prim_path, cfg=cfg) + # make the asset uninstanceable (in case it is) + # point instancer defines its own prototypes so if an asset is already instanced, this doesn't work. + self._process_prototype_prim(marker_prim) + # add child reference to point instancer + self._instancer_manager.GetPrototypesRel().AddTarget(marker_prim_path) + # check that we loaded all the prototypes + prototypes = self._instancer_manager.GetPrototypesRel().GetTargets() + if len(prototypes) != len(markers_cfg): + raise RuntimeError( + f"Failed to load all the prototypes. Expected: {len(markers_cfg)}. Received: {len(prototypes)}." + ) + + def _process_prototype_prim(self, prim: Usd.Prim): + """Process a prim and its descendants to make them suitable for defining prototypes. + + Point instancer defines its own prototypes so if an asset is already instanced, this doesn't work. + This function checks if the prim at the specified prim path and its descendants are instanced. + If so, it makes the respective prim uninstanceable by disabling instancing on the prim. + + Additionally, it makes the prim invisible to secondary rays. This is useful when we do not want + to see the marker prims on camera images. + + Args: + prim_path: The prim path to check. + stage: The stage where the prim exists. + Defaults to None, in which case the current stage is used. + """ + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPrimAtPath()}' is not valid.") + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # check if it is physics body -> if so, remove it + if child_prim.HasAPI(UsdPhysics.ArticulationRootAPI): + child_prim.RemoveAPI(UsdPhysics.ArticulationRootAPI) + child_prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI) + if child_prim.HasAPI(UsdPhysics.RigidBodyAPI): + child_prim.RemoveAPI(UsdPhysics.RigidBodyAPI) + child_prim.RemoveAPI(PhysxSchema.PhysxRigidBodyAPI) + if child_prim.IsA(UsdPhysics.Joint): + child_prim.GetAttribute("physics:jointEnabled").Set(False) + # check if prim is instanced -> if so, make it uninstanceable + if child_prim.IsInstance(): + child_prim.SetInstanceable(False) + # check if prim is a mesh -> if so, make it invisible to secondary rays + if child_prim.IsA(UsdGeom.Gprim): + # invisible to secondary rays such as depth images + omni.kit.commands.execute( + "ChangePropertyCommand", + prop_path=Sdf.Path(f"{child_prim.GetPrimPath().pathString}.primvars:invisibleToSecondaryRays"), + value=True, + prev=None, + type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, + ) + # add children to list + all_prims += child_prim.GetChildren() + + # remove any physics on the markers because they are only for visualization! + physx_utils.removeRigidBodySubtree(prim) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/__init__.py new file mode 100644 index 00000000000..3de8043a50d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package containing an interactive scene definition. + +A scene is a collection of entities (e.g., terrain, articulations, sensors, lights, etc.) that can be added to the +simulation. However, only a subset of these entities are of direct interest for the user to interact with. +For example, the user may want to interact with a robot in the scene, but not with the terrain or the lights. +For this reason, we integrate the different entities into a single class called :class:`InteractiveScene`. + +The interactive scene performs the following tasks: + +1. It parses the configuration class :class:`InteractiveSceneCfg` to create the scene. This configuration class is + inherited by the user to add entities to the scene. +2. It clones the entities based on the number of environments specified by the user. +3. It clubs the entities into different groups based on their type (e.g., articulations, sensors, etc.). +4. It provides a set of methods to unify the common operations on the entities in the scene (e.g., resetting internal + buffers, writing buffers to simulation and updating buffers from simulation). + +The interactive scene can be passed around to different modules in the framework to perform different tasks. +For instance, computing the observations based on the state of the scene, or randomizing the scene, or applying +actions to the scene. All these are handled by different "managers" in the framework. Please refer to the +:mod:`isaaclab.managers` sub-package for more details. +""" + +from .interactive_scene import InteractiveScene +from .interactive_scene_cfg import InteractiveSceneCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene.py new file mode 100644 index 00000000000..9f21f673996 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene.py @@ -0,0 +1,706 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from collections.abc import Sequence +from typing import Any + +import carb +import omni.log +import omni.usd +from isaacsim.core.cloner import GridCloner +from isaacsim.core.prims import XFormPrim +from pxr import PhysxSchema + +import isaaclab.sim as sim_utils +from isaaclab.assets import ( + Articulation, + ArticulationCfg, + AssetBaseCfg, + DeformableObject, + DeformableObjectCfg, + RigidObject, + RigidObjectCfg, + RigidObjectCollection, + RigidObjectCollectionCfg, +) +from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg +from isaaclab.terrains import TerrainImporter, TerrainImporterCfg + +from .interactive_scene_cfg import InteractiveSceneCfg + + +class InteractiveScene: + """A scene that contains entities added to the simulation. + + The interactive scene parses the :class:`InteractiveSceneCfg` class to create the scene. + Based on the specified number of environments, it clones the entities and groups them into different + categories (e.g., articulations, sensors, etc.). + + Cloning can be performed in two ways: + + * For tasks where all environments contain the same assets, a more performant cloning paradigm + can be used to allow for faster environment creation. This is specified by the ``replicate_physics`` flag. + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=True)) + + * For tasks that require having separate assets in the environments, ``replicate_physics`` would have to + be set to False, which will add some costs to the overall startup time. + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=False)) + + Each entity is registered to scene based on its name in the configuration class. For example, if the user + specifies a robot in the configuration class as follows: + + .. code-block:: python + + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.utils import configclass + + from isaaclab_assets.robots.anymal import ANYMAL_C_CFG + + @configclass + class MySceneCfg(InteractiveSceneCfg): + + robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + Then the robot can be accessed from the scene as follows: + + .. code-block:: python + + from isaaclab.scene import InteractiveScene + + # create 128 environments + scene = InteractiveScene(cfg=MySceneCfg(num_envs=128)) + + # access the robot from the scene + robot = scene["robot"] + # access the robot based on its type + robot = scene.articulations["robot"] + + If the :class:`InteractiveSceneCfg` class does not include asset entities, the cloning process + can still be triggered if assets were added to the stage outside of the :class:`InteractiveScene` class: + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(num_envs=128, replicate_physics=True)) + scene.clone_environments() + + .. note:: + It is important to note that the scene only performs common operations on the entities. For example, + resetting the internal buffers, writing the buffers to the simulation and updating the buffers from the + simulation. The scene does not perform any task specific to the entity. For example, it does not apply + actions to the robot or compute observations from the robot. These tasks are handled by different + modules called "managers" in the framework. Please refer to the :mod:`isaaclab.managers` sub-package + for more details. + """ + + def __init__(self, cfg: InteractiveSceneCfg): + """Initializes the scene. + + Args: + cfg: The configuration class for the scene. + """ + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg + # initialize scene elements + self._terrain = None + self._articulations = dict() + self._deformable_objects = dict() + self._rigid_objects = dict() + self._rigid_object_collections = dict() + self._sensors = dict() + self._extras = dict() + # obtain the current stage + self.stage = omni.usd.get_context().get_stage() + # physics scene path + self._physics_scene_path = None + # prepare cloner for environment replication + self.cloner = GridCloner(spacing=self.cfg.env_spacing) + self.cloner.define_base_env(self.env_ns) + self.env_prim_paths = self.cloner.generate_paths(f"{self.env_ns}/env", self.cfg.num_envs) + # create source prim + self.stage.DefinePrim(self.env_prim_paths[0], "Xform") + + # when replicate_physics=False, we assume heterogeneous environments and clone the xforms first. + # this triggers per-object level cloning in the spawner. + if not self.cfg.replicate_physics: + # clone the env xform + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=False, + copy_from_source=True, + enable_env_ids=self.cfg.filter_collisions, # this won't do anything because we are not replicating physics + ) + self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + else: + # otherwise, environment origins will be initialized during cloning at the end of environment creation + self._default_env_origins = None + + self._global_prim_paths = list() + if self._is_scene_setup_from_cfg(): + # add entities from config + self._add_entities_from_cfg() + # clone environments on a global scope if environment is homogeneous + if self.cfg.replicate_physics: + self.clone_environments(copy_from_source=False) + # replicate physics if we have more than one environment + # this is done to make scene initialization faster at play time + if self.cfg.replicate_physics and self.cfg.num_envs > 1: + self.cloner.replicate_physics( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + base_env_path=self.env_ns, + root_path=self.env_regex_ns.replace(".*", ""), + enable_env_ids=self.cfg.filter_collisions, + ) + + # since env_ids is only applicable when replicating physics, we have to fallback to the previous method + # to filter collisions if replicate_physics is not enabled + if not self.cfg.replicate_physics and self.cfg.filter_collisions: + self.filter_collisions(self._global_prim_paths) + + def clone_environments(self, copy_from_source: bool = False): + """Creates clones of the environment ``/World/envs/env_0``. + + Args: + copy_from_source: (bool): If set to False, clones inherit from /World/envs/env_0 and mirror its changes. + If True, clones are independent copies of the source prim and won't reflect its changes (start-up time + may increase). Defaults to False. + """ + # check if user spawned different assets in individual environments + # this flag will be None if no multi asset is spawned + carb_settings_iface = carb.settings.get_settings() + has_multi_assets = carb_settings_iface.get("/isaaclab/spawn/multi_assets") + if has_multi_assets and self.cfg.replicate_physics: + omni.log.warn( + "Varying assets might have been spawned under different environments." + " However, the replicate physics flag is enabled in the 'InteractiveScene' configuration." + " This may adversely affect PhysX parsing. We recommend disabling this property." + ) + + # clone the environment + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=self.cfg.replicate_physics, + copy_from_source=copy_from_source, + enable_env_ids=self.cfg.filter_collisions, # this automatically filters collisions between environments + ) + + # since env_ids is only applicable when replicating physics, we have to fallback to the previous method + # to filter collisions if replicate_physics is not enabled + if not self.cfg.replicate_physics and self.cfg.filter_collisions: + omni.log.warn( + "Collision filtering can only be automatically enabled when replicate_physics=True." + " Please call scene.filter_collisions(global_prim_paths) to filter collisions across environments." + ) + + # in case of heterogeneous cloning, the env origins is specified at init + if self._default_env_origins is None: + self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + + def filter_collisions(self, global_prim_paths: list[str] | None = None): + """Filter environments collisions. + + Disables collisions between the environments in ``/World/envs/env_.*`` and enables collisions with the prims + in global prim paths (e.g. ground plane). + + Args: + global_prim_paths: A list of global prim paths to enable collisions with. + Defaults to None, in which case no global prim paths are considered. + """ + # validate paths in global prim paths + if global_prim_paths is None: + global_prim_paths = [] + else: + # remove duplicates in paths + global_prim_paths = list(set(global_prim_paths)) + + # set global prim paths list if not previously defined + if len(self._global_prim_paths) < 1: + self._global_prim_paths += global_prim_paths + + # filter collisions within each environment instance + self.cloner.filter_collisions( + self.physics_scene_path, + "/World/collisions", + self.env_prim_paths, + global_paths=self._global_prim_paths, + ) + + def __str__(self) -> str: + """Returns a string representation of the scene.""" + msg = f"\n" + msg += f"\tNumber of environments: {self.cfg.num_envs}\n" + msg += f"\tEnvironment spacing : {self.cfg.env_spacing}\n" + msg += f"\tSource prim name : {self.env_prim_paths[0]}\n" + msg += f"\tGlobal prim paths : {self._global_prim_paths}\n" + msg += f"\tReplicate physics : {self.cfg.replicate_physics}" + return msg + + """ + Properties. + """ + + @property + def physics_scene_path(self) -> str: + """The path to the USD Physics Scene.""" + if self._physics_scene_path is None: + for prim in self.stage.Traverse(): + if prim.HasAPI(PhysxSchema.PhysxSceneAPI): + self._physics_scene_path = prim.GetPrimPath().pathString + omni.log.info(f"Physics scene prim path: {self._physics_scene_path}") + break + if self._physics_scene_path is None: + raise RuntimeError("No physics scene found! Please make sure one exists.") + return self._physics_scene_path + + @property + def physics_dt(self) -> float: + """The physics timestep of the scene.""" + return sim_utils.SimulationContext.instance().get_physics_dt() # pyright: ignore [reportOptionalMemberAccess] + + @property + def device(self) -> str: + """The device on which the scene is created.""" + return sim_utils.SimulationContext.instance().device # pyright: ignore [reportOptionalMemberAccess] + + @property + def env_ns(self) -> str: + """The namespace ``/World/envs`` in which all environments created. + + The environments are present w.r.t. this namespace under "env_{N}" prim, + where N is a natural number. + """ + return "/World/envs" + + @property + def env_regex_ns(self) -> str: + """The namespace ``/World/envs/env_.*`` in which all environments created.""" + return f"{self.env_ns}/env_.*" + + @property + def num_envs(self) -> int: + """The number of environments handled by the scene.""" + return self.cfg.num_envs + + @property + def env_origins(self) -> torch.Tensor: + """The origins of the environments in the scene. Shape is (num_envs, 3).""" + if self._terrain is not None: + return self._terrain.env_origins + else: + return self._default_env_origins + + @property + def terrain(self) -> TerrainImporter | None: + """The terrain in the scene. If None, then the scene has no terrain. + + Note: + We treat terrain separate from :attr:`extras` since terrains define environment origins and are + handled differently from other miscellaneous entities. + """ + return self._terrain + + @property + def articulations(self) -> dict[str, Articulation]: + """A dictionary of articulations in the scene.""" + return self._articulations + + @property + def deformable_objects(self) -> dict[str, DeformableObject]: + """A dictionary of deformable objects in the scene.""" + return self._deformable_objects + + @property + def rigid_objects(self) -> dict[str, RigidObject]: + """A dictionary of rigid objects in the scene.""" + return self._rigid_objects + + @property + def rigid_object_collections(self) -> dict[str, RigidObjectCollection]: + """A dictionary of rigid object collections in the scene.""" + return self._rigid_object_collections + + @property + def sensors(self) -> dict[str, SensorBase]: + """A dictionary of the sensors in the scene, such as cameras and contact reporters.""" + return self._sensors + + @property + def extras(self) -> dict[str, XFormPrim]: + """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. + + The keys are the names of the miscellaneous objects, and the values are the `XFormPrim`_ + of the corresponding prims. + + As an example, lights or other props in the scene that do not have any attributes or properties that you + want to alter at runtime can be added to this dictionary. + + Note: + These are not reset or updated by the scene. They are mainly other prims that are not necessarily + handled by the interactive scene, but are useful to be accessed by the user. + + .. _XFormPrim: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.core/docs/index.html#isaacsim.core.prims.XFormPrim + + """ + return self._extras + + @property + def state(self) -> dict[str, dict[str, dict[str, torch.Tensor]]]: + """A dictionary of the state of the scene entities in the simulation world frame. + + Please refer to :meth:`get_state` for the format. + """ + return self.get_state(is_relative=False) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the scene entities. + + Args: + env_ids: The indices of the environments to reset. + Defaults to None (all instances). + """ + # -- assets + for articulation in self._articulations.values(): + articulation.reset(env_ids) + for deformable_object in self._deformable_objects.values(): + deformable_object.reset(env_ids) + for rigid_object in self._rigid_objects.values(): + rigid_object.reset(env_ids) + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.reset(env_ids) + # -- sensors + for sensor in self._sensors.values(): + sensor.reset(env_ids) + + def write_data_to_sim(self): + """Writes the data of the scene entities to the simulation.""" + # -- assets + for articulation in self._articulations.values(): + articulation.write_data_to_sim() + for deformable_object in self._deformable_objects.values(): + deformable_object.write_data_to_sim() + for rigid_object in self._rigid_objects.values(): + rigid_object.write_data_to_sim() + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.write_data_to_sim() + + def update(self, dt: float) -> None: + """Update the scene entities. + + Args: + dt: The amount of time passed from last :meth:`update` call. + """ + # -- assets + for articulation in self._articulations.values(): + articulation.update(dt) + for deformable_object in self._deformable_objects.values(): + deformable_object.update(dt) + for rigid_object in self._rigid_objects.values(): + rigid_object.update(dt) + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.update(dt) + # -- sensors + for sensor in self._sensors.values(): + sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) + + """ + Operations: Scene State. + """ + + def reset_to( + self, + state: dict[str, dict[str, dict[str, torch.Tensor]]], + env_ids: Sequence[int] | None = None, + is_relative: bool = False, + ): + """Resets the entities in the scene to the provided state. + + Args: + state: The state to reset the scene entities to. Please refer to :meth:`get_state` for the format. + env_ids: The indices of the environments to reset. Defaults to None, in which case + all environment instances are reset. + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + """ + # resolve env_ids + if env_ids is None: + env_ids = slice(None) + # articulations + for asset_name, articulation in self._articulations.items(): + asset_state = state["articulation"][asset_name] + # root state + root_pose = asset_state["root_pose"].clone() + if is_relative: + root_pose[:, :3] += self.env_origins[env_ids] + root_velocity = asset_state["root_velocity"].clone() + articulation.write_root_pose_to_sim(root_pose, env_ids=env_ids) + articulation.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) + # joint state + joint_position = asset_state["joint_position"].clone() + joint_velocity = asset_state["joint_velocity"].clone() + articulation.write_joint_state_to_sim(joint_position, joint_velocity, env_ids=env_ids) + # FIXME: This is not generic as it assumes PD control over the joints. + # This assumption does not hold for effort controlled joints. + articulation.set_joint_position_target(joint_position, env_ids=env_ids) + articulation.set_joint_velocity_target(joint_velocity, env_ids=env_ids) + # deformable objects + for asset_name, deformable_object in self._deformable_objects.items(): + asset_state = state["deformable_object"][asset_name] + nodal_position = asset_state["nodal_position"].clone() + if is_relative: + nodal_position[:, :3] += self.env_origins[env_ids] + nodal_velocity = asset_state["nodal_velocity"].clone() + deformable_object.write_nodal_pos_to_sim(nodal_position, env_ids=env_ids) + deformable_object.write_nodal_velocity_to_sim(nodal_velocity, env_ids=env_ids) + # rigid objects + for asset_name, rigid_object in self._rigid_objects.items(): + asset_state = state["rigid_object"][asset_name] + root_pose = asset_state["root_pose"].clone() + if is_relative: + root_pose[:, :3] += self.env_origins[env_ids] + root_velocity = asset_state["root_velocity"].clone() + rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids) + rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) + + # write data to simulation to make sure initial state is set + # this propagates the joint targets to the simulation + self.write_data_to_sim() + + def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, torch.Tensor]]]: + """Returns the state of the scene entities. + + Based on the type of the entity, the state comprises of different components. + + * For an articulation, the state comprises of the root pose, root velocity, and joint position and velocity. + * For a deformable object, the state comprises of the nodal position and velocity. + * For a rigid object, the state comprises of the root pose and root velocity. + + The returned state is a dictionary with the following format: + + .. code-block:: python + + { + "articulation": { + "entity_1_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + "joint_position": torch.Tensor, + "joint_velocity": torch.Tensor, + }, + "entity_2_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + "joint_position": torch.Tensor, + "joint_velocity": torch.Tensor, + }, + }, + "deformable_object": { + "entity_3_name": { + "nodal_position": torch.Tensor, + "nodal_velocity": torch.Tensor, + } + }, + "rigid_object": { + "entity_4_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + } + }, + } + + where ``entity_N_name`` is the name of the entity registered in the scene. + + Args: + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + + Returns: + A dictionary of the state of the scene entities. + """ + state = dict() + # articulations + state["articulation"] = dict() + for asset_name, articulation in self._articulations.items(): + asset_state = dict() + asset_state["root_pose"] = articulation.data.root_pose_w.clone() + if is_relative: + asset_state["root_pose"][:, :3] -= self.env_origins + asset_state["root_velocity"] = articulation.data.root_vel_w.clone() + asset_state["joint_position"] = articulation.data.joint_pos.clone() + asset_state["joint_velocity"] = articulation.data.joint_vel.clone() + state["articulation"][asset_name] = asset_state + # deformable objects + state["deformable_object"] = dict() + for asset_name, deformable_object in self._deformable_objects.items(): + asset_state = dict() + asset_state["nodal_position"] = deformable_object.data.nodal_pos_w.clone() + if is_relative: + asset_state["nodal_position"][:, :3] -= self.env_origins + asset_state["nodal_velocity"] = deformable_object.data.nodal_vel_w.clone() + state["deformable_object"][asset_name] = asset_state + # rigid objects + state["rigid_object"] = dict() + for asset_name, rigid_object in self._rigid_objects.items(): + asset_state = dict() + asset_state["root_pose"] = rigid_object.data.root_pose_w.clone() + if is_relative: + asset_state["root_pose"][:, :3] -= self.env_origins + asset_state["root_velocity"] = rigid_object.data.root_vel_w.clone() + state["rigid_object"][asset_name] = asset_state + return state + + """ + Operations: Iteration. + """ + + def keys(self) -> list[str]: + """Returns the keys of the scene entities. + + Returns: + The keys of the scene entities. + """ + all_keys = ["terrain"] + for asset_family in [ + self._articulations, + self._deformable_objects, + self._rigid_objects, + self._rigid_object_collections, + self._sensors, + self._extras, + ]: + all_keys += list(asset_family.keys()) + return all_keys + + def __getitem__(self, key: str) -> Any: + """Returns the scene entity with the given key. + + Args: + key: The key of the scene entity. + + Returns: + The scene entity. + """ + # check if it is a terrain + if key == "terrain": + return self._terrain + + all_keys = ["terrain"] + # check if it is in other dictionaries + for asset_family in [ + self._articulations, + self._deformable_objects, + self._rigid_objects, + self._rigid_object_collections, + self._sensors, + self._extras, + ]: + out = asset_family.get(key) + # if found, return + if out is not None: + return out + all_keys += list(asset_family.keys()) + # if not found, raise error + raise KeyError(f"Scene entity with key '{key}' not found. Available Entities: '{all_keys}'") + + """ + Internal methods. + """ + + def _is_scene_setup_from_cfg(self) -> bool: + """Check if scene entities are setup from the config or not. + + Returns: + True if scene entities are setup from the config, False otherwise. + """ + return any( + not (asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None) + for asset_name, asset_cfg in self.cfg.__dict__.items() + ) + + def _add_entities_from_cfg(self): + """Add scene entities from the config.""" + # store paths that are in global collision filter + self._global_prim_paths = list() + # parse the entire scene config and resolve regex + for asset_name, asset_cfg in self.cfg.__dict__.items(): + # skip keywords + # note: easier than writing a list of keywords: [num_envs, env_spacing, lazy_sensor_update] + if asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None: + continue + # resolve regex + if hasattr(asset_cfg, "prim_path"): + asset_cfg.prim_path = asset_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + # create asset + if isinstance(asset_cfg, TerrainImporterCfg): + # terrains are special entities since they define environment origins + asset_cfg.num_envs = self.cfg.num_envs + asset_cfg.env_spacing = self.cfg.env_spacing + self._terrain = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, ArticulationCfg): + self._articulations[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, DeformableObjectCfg): + self._deformable_objects[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, RigidObjectCfg): + self._rigid_objects[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, RigidObjectCollectionCfg): + for rigid_object_cfg in asset_cfg.rigid_objects.values(): + rigid_object_cfg.prim_path = rigid_object_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + self._rigid_object_collections[asset_name] = asset_cfg.class_type(asset_cfg) + for rigid_object_cfg in asset_cfg.rigid_objects.values(): + if hasattr(rigid_object_cfg, "collision_group") and rigid_object_cfg.collision_group == -1: + asset_paths = sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path) + self._global_prim_paths += asset_paths + elif isinstance(asset_cfg, SensorBaseCfg): + # Update target frame path(s)' regex name space for FrameTransformer + if isinstance(asset_cfg, FrameTransformerCfg): + updated_target_frames = [] + for target_frame in asset_cfg.target_frames: + target_frame.prim_path = target_frame.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + updated_target_frames.append(target_frame) + asset_cfg.target_frames = updated_target_frames + elif isinstance(asset_cfg, ContactSensorCfg): + updated_filter_prim_paths_expr = [] + for filter_prim_path in asset_cfg.filter_prim_paths_expr: + updated_filter_prim_paths_expr.append(filter_prim_path.format(ENV_REGEX_NS=self.env_regex_ns)) + asset_cfg.filter_prim_paths_expr = updated_filter_prim_paths_expr + + self._sensors[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, AssetBaseCfg): + # manually spawn asset + if asset_cfg.spawn is not None: + asset_cfg.spawn.func( + asset_cfg.prim_path, + asset_cfg.spawn, + translation=asset_cfg.init_state.pos, + orientation=asset_cfg.init_state.rot, + ) + # store xform prim view corresponding to this asset + # all prims in the scene are Xform prims (i.e. have a transform component) + self._extras[asset_name] = XFormPrim(asset_cfg.prim_path, reset_xform_properties=False) + else: + raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") + # store global collision paths + if hasattr(asset_cfg, "collision_group") and asset_cfg.collision_group == -1: + asset_paths = sim_utils.find_matching_prim_paths(asset_cfg.prim_path) + self._global_prim_paths += asset_paths diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene_cfg.py new file mode 100644 index 00000000000..46e6501f8fd --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/scene/interactive_scene_cfg.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils.configclass import configclass + + +@configclass +class InteractiveSceneCfg: + """Configuration for the interactive scene. + + The users can inherit from this class to add entities to their scene. This is then parsed by the + :class:`InteractiveScene` class to create the scene. + + .. note:: + The adding of entities to the scene is sensitive to the order of the attributes in the configuration. + Please make sure to add the entities in the order you want them to be added to the scene. + The recommended order of specification is terrain, physics-related assets (articulations and rigid bodies), + sensors and non-physics-related assets (lights). + + For example, to add a robot to the scene, the user can create a configuration class as follows: + + .. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab.assets import AssetBaseCfg + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.sensors.ray_caster import GridPatternCfg, RayCasterCfg + from isaaclab.utils import configclass + + from isaaclab_assets.robots.anymal import ANYMAL_C_CFG + + @configclass + class MySceneCfg(InteractiveSceneCfg): + + # terrain - flat terrain plane + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + ) + + # articulation - robot 1 + robot_1 = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot_1") + # articulation - robot 2 + robot_2 = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot_2") + robot_2.init_state.pos = (0.0, 1.0, 0.6) + + # sensor - ray caster attached to the base of robot 1 that scans the ground + height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot_1/base", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + attach_yaw_only=True, + pattern_cfg=GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=True, + mesh_prim_paths=["/World/ground"], + ) + + # extras - light + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 500.0)), + ) + + """ + + num_envs: int = MISSING + """Number of environment instances handled by the scene.""" + + env_spacing: float = MISSING + """Spacing between environments. + + This is the default distance between environment origins in the scene. Used only when the + number of environments is greater than one. + """ + + lazy_sensor_update: bool = True + """Whether to update sensors only when they are accessed. Default is True. + + If true, the sensor data is only updated when their attribute ``data`` is accessed. Otherwise, the sensor + data is updated every time sensors are updated. + """ + + replicate_physics: bool = True + """Enable/disable replication of physics schemas when using the Cloner APIs. Default is True. + + If True, the simulation will have the same asset instances (USD prims) in all the cloned environments. + Internally, this ensures optimization in setting up the scene and parsing it via the physics stage parser. + + If False, the simulation allows having separate asset instances (USD prims) in each environment. + This flexibility comes at a cost of slowdowns in setting up and parsing the scene. + + .. note:: + Optimized parsing of certain prim types (such as deformable objects) is not currently supported + by the physics engine. In these cases, this flag needs to be set to False. + """ + + filter_collisions: bool = True + """Enable/disable collision filtering between cloned environments. Default is True. + + If True, collisions will not occur between cloned environments. + + If False, the simulation will generate collisions between environments. + + .. note:: + Collisions can only be filtered automatically in direct workflows when physics replication is enabled. + If ``replicated_physics=False`` and collision filtering is desired, make sure to call ``scene.filter_collisions()``. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/__init__.py new file mode 100644 index 00000000000..802a85cea42 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/__init__.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package containing various sensor classes implementations. + +This subpackage contains the sensor classes that are compatible with Isaac Sim. We include both +USD-based and custom sensors: + +* **USD-prim sensors**: Available in Omniverse and require creating a USD prim for them. + For instance, RTX ray tracing camera and lidar sensors. +* **USD-schema sensors**: Available in Omniverse and require creating a USD schema on an existing prim. + For instance, contact sensors and frame transformers. +* **Custom sensors**: Implemented in Python and do not require creating any USD prim or schema. + For instance, warp-based ray-casters. + +Due to the above categorization, the prim paths passed to the sensor's configuration class +are interpreted differently based on the sensor type. The following table summarizes the +interpretation of the prim paths for different sensor types: + ++---------------------+---------------------------+---------------------------------------------------------------+ +| Sensor Type | Example Prim Path | Pre-check | ++=====================+===========================+===============================================================+ +| Camera | /World/robot/base/camera | Leaf is available, and it will spawn a USD camera | ++---------------------+---------------------------+---------------------------------------------------------------+ +| Contact Sensor | /World/robot/feet_* | Leaf is available and checks if the schema exists | ++---------------------+---------------------------+---------------------------------------------------------------+ +| Ray Caster | /World/robot/base | Leaf exists and is a physics body (Articulation / Rigid Body) | ++---------------------+---------------------------+---------------------------------------------------------------+ +| Frame Transformer | /World/robot/base | Leaf exists and is a physics body (Articulation / Rigid Body) | ++---------------------+---------------------------+---------------------------------------------------------------+ +| Imu | /World/robot/base | Leaf exists and is a physics body (Rigid Body) | ++---------------------+---------------------------+---------------------------------------------------------------+ + +""" + +from .camera import * # noqa: F401, F403 +from .contact_sensor import * # noqa: F401, F403 +from .frame_transformer import * # noqa: F401 +from .imu import * # noqa: F401, F403 +from .ray_caster import * # noqa: F401, F403 +from .sensor_base import SensorBase # noqa: F401 +from .sensor_base_cfg import SensorBaseCfg # noqa: F401 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/__init__.py new file mode 100644 index 00000000000..1fa24f83f05 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for camera wrapper around USD camera prim.""" + +from .camera import Camera +from .camera_cfg import CameraCfg +from .camera_data import CameraData +from .tiled_camera import TiledCamera +from .tiled_camera_cfg import TiledCameraCfg +from .utils import * # noqa: F401, F403 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera.py new file mode 100644 index 00000000000..90e0fa7f9f9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera.py @@ -0,0 +1,727 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import json +import numpy as np +import re +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any, Literal + +import carb +import isaacsim.core.utils.stage as stage_utils +import omni.kit.commands +import omni.usd +from isaacsim.core.prims import XFormPrim +from isaacsim.core.version import get_version +from pxr import Sdf, UsdGeom + +import isaaclab.sim as sim_utils +import isaaclab.utils.sensors as sensor_utils +from isaaclab.utils import to_camel_case +from isaaclab.utils.array import convert_to_torch +from isaaclab.utils.math import ( + convert_camera_frame_orientation_convention, + create_rotation_matrix_from_view, + quat_from_matrix, +) + +from ..sensor_base import SensorBase +from .camera_data import CameraData + +if TYPE_CHECKING: + from .camera_cfg import CameraCfg + + +class Camera(SensorBase): + r"""The camera sensor for acquiring visual data. + + This class wraps over the `UsdGeom Camera`_ for providing a consistent API for acquiring visual data. + It ensures that the camera follows the ROS convention for the coordinate system. + + Summarizing from the `replicator extension`_, the following sensor types are supported: + + - ``"rgb"``: A 3-channel rendered color image. + - ``"rgba"``: A 4-channel rendered color image with alpha channel. + - ``"distance_to_camera"``: An image containing the distance to camera optical center. + - ``"distance_to_image_plane"``: An image containing distances of 3D points from camera plane along camera's z-axis. + - ``"depth"``: The same as ``"distance_to_image_plane"``. + - ``"normals"``: An image containing the local surface normal vectors at each pixel. + - ``"motion_vectors"``: An image containing the motion vector data at each pixel. + - ``"semantic_segmentation"``: The semantic segmentation data. + - ``"instance_segmentation_fast"``: The instance segmentation data. + - ``"instance_id_segmentation_fast"``: The instance id segmentation data. + + .. note:: + Currently the following sensor types are not supported in a "view" format: + + - ``"instance_segmentation"``: The instance segmentation data. Please use the fast counterparts instead. + - ``"instance_id_segmentation"``: The instance id segmentation data. Please use the fast counterparts instead. + - ``"bounding_box_2d_tight"``: The tight 2D bounding box data (only contains non-occluded regions). + - ``"bounding_box_2d_tight_fast"``: The tight 2D bounding box data (only contains non-occluded regions). + - ``"bounding_box_2d_loose"``: The loose 2D bounding box data (contains occluded regions). + - ``"bounding_box_2d_loose_fast"``: The loose 2D bounding box data (contains occluded regions). + - ``"bounding_box_3d"``: The 3D view space bounding box data. + - ``"bounding_box_3d_fast"``: The 3D view space bounding box data. + + .. _replicator extension: https://docs.omniverse.nvidia.com/extensions/latest/ext_replicator/annotators_details.html#annotator-output + .. _USDGeom Camera: https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html + + """ + + cfg: CameraCfg + """The configuration parameters.""" + + UNSUPPORTED_TYPES: set[str] = { + "instance_id_segmentation", + "instance_segmentation", + "bounding_box_2d_tight", + "bounding_box_2d_loose", + "bounding_box_3d", + "bounding_box_2d_tight_fast", + "bounding_box_2d_loose_fast", + "bounding_box_3d_fast", + } + """The set of sensor types that are not supported by the camera class.""" + + def __init__(self, cfg: CameraCfg): + """Initializes the camera sensor. + + Args: + cfg: The configuration parameters. + + Raises: + RuntimeError: If no camera prim is found at the given path. + ValueError: If the provided data types are not supported by the camera. + """ + # check if sensor path is valid + # note: currently we do not handle environment indices if there is a regex pattern in the leaf + # For example, if the prim path is "/World/Sensor_[1,2]". + sensor_path = cfg.prim_path.split("/")[-1] + sensor_path_is_regex = re.match(r"^[a-zA-Z0-9/_]+$", sensor_path) is None + if sensor_path_is_regex: + raise RuntimeError( + f"Invalid prim path for the camera sensor: {self.cfg.prim_path}." + "\n\tHint: Please ensure that the prim path does not contain any regex patterns in the leaf." + ) + # perform check on supported data types + self._check_supported_data_types(cfg) + # initialize base class + super().__init__(cfg) + + # toggle rendering of rtx sensors as True + # this flag is read by SimulationContext to determine if rtx sensors should be rendered + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/isaaclab/render/rtx_sensors", True) + + # spawn the asset + if self.cfg.spawn is not None: + # compute the rotation offset + rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32, device="cpu").unsqueeze(0) + rot_offset = convert_camera_frame_orientation_convention( + rot, origin=self.cfg.offset.convention, target="opengl" + ) + rot_offset = rot_offset.squeeze(0).cpu().numpy() + # ensure vertical aperture is set, otherwise replace with default for squared pixels + if self.cfg.spawn.vertical_aperture is None: + self.cfg.spawn.vertical_aperture = self.cfg.spawn.horizontal_aperture * self.cfg.height / self.cfg.width + # spawn the asset + self.cfg.spawn.func( + self.cfg.prim_path, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=rot_offset + ) + # check that spawn was successful + matching_prims = sim_utils.find_matching_prims(self.cfg.prim_path) + if len(matching_prims) == 0: + raise RuntimeError(f"Could not find prim with path {self.cfg.prim_path}.") + + # UsdGeom Camera prim for the sensor + self._sensor_prims: list[UsdGeom.Camera] = list() + # Create empty variables for storing output data + self._data = CameraData() + + # HACK: we need to disable instancing for semantic_segmentation and instance_segmentation_fast to work + isaac_sim_version = get_version() + # checks for Isaac Sim v4.5 as this issue exists there + if int(isaac_sim_version[2]) == 4 and int(isaac_sim_version[3]) == 5: + if "semantic_segmentation" in self.cfg.data_types or "instance_segmentation_fast" in self.cfg.data_types: + omni.log.warn( + "Isaac Sim 4.5 introduced a bug in Camera and TiledCamera when outputting instance and semantic" + " segmentation outputs for instanceable assets. As a workaround, the instanceable flag on assets" + " will be disabled in the current workflow and may lead to longer load times and increased memory" + " usage." + ) + stage = omni.usd.get_context().get_stage() + with Sdf.ChangeBlock(): + for prim in stage.Traverse(): + prim.SetInstanceable(False) + + def __del__(self): + """Unsubscribes from callbacks and detach from the replicator registry.""" + # unsubscribe callbacks + super().__del__() + # delete from replicator registry + for _, annotators in self._rep_registry.items(): + for annotator, render_product_path in zip(annotators, self._render_product_paths): + annotator.detach([render_product_path]) + annotator = None + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + # message for class + return ( + f"Camera @ '{self.cfg.prim_path}': \n" + f"\tdata types : {list(self.data.output.keys())} \n" + f"\tsemantic filter : {self.cfg.semantic_filter}\n" + f"\tcolorize semantic segm. : {self.cfg.colorize_semantic_segmentation}\n" + f"\tcolorize instance segm. : {self.cfg.colorize_instance_segmentation}\n" + f"\tcolorize instance id segm.: {self.cfg.colorize_instance_id_segmentation}\n" + f"\tupdate period (s): {self.cfg.update_period}\n" + f"\tshape : {self.image_shape}\n" + f"\tnumber of sensors : {self._view.count}" + ) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self._view.count + + @property + def data(self) -> CameraData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def frame(self) -> torch.tensor: + """Frame number when the measurement took place.""" + return self._frame + + @property + def render_product_paths(self) -> list[str]: + """The path of the render products for the cameras. + + This can be used via replicator interfaces to attach to writes or external annotator registry. + """ + return self._render_product_paths + + @property + def image_shape(self) -> tuple[int, int]: + """A tuple containing (height, width) of the camera sensor.""" + return (self.cfg.height, self.cfg.width) + + """ + Configuration + """ + + def set_intrinsic_matrices( + self, matrices: torch.Tensor, focal_length: float | None = None, env_ids: Sequence[int] | None = None + ): + """Set parameters of the USD camera from its intrinsic matrix. + + The intrinsic matrix is used to set the following parameters to the USD camera: + + - ``focal_length``: The focal length of the camera. + - ``horizontal_aperture``: The horizontal aperture of the camera. + - ``vertical_aperture``: The vertical aperture of the camera. + - ``horizontal_aperture_offset``: The horizontal offset of the camera. + - ``vertical_aperture_offset``: The vertical offset of the camera. + + .. warning:: + + Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, + i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption + is not true in the input intrinsic matrix, then the camera will not set up correctly. + + Args: + matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None, + focal_length will be calculated 1 / width. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # convert matrices to numpy tensors + if isinstance(matrices, torch.Tensor): + matrices = matrices.cpu().numpy() + else: + matrices = np.asarray(matrices, dtype=float) + # iterate over env_ids + for i, intrinsic_matrix in zip(env_ids, matrices): + + height, width = self.image_shape + + params = sensor_utils.convert_camera_intrinsics_to_usd( + intrinsic_matrix=intrinsic_matrix.reshape(-1), height=height, width=width, focal_length=focal_length + ) + + # change data for corresponding camera index + sensor_prim = self._sensor_prims[i] + # set parameters for camera + for param_name, param_value in params.items(): + # convert to camel case (CC) + param_name = to_camel_case(param_name, to="CC") + # get attribute from the class + param_attr = getattr(sensor_prim, f"Get{param_name}Attr") + # set value + # note: We have to do it this way because the camera might be on a different + # layer (default cameras are on session layer), and this is the simplest + # way to set the property on the right layer. + omni.usd.set_prop_val(param_attr(), param_value) + # update the internal buffers + self._update_intrinsic_matrices(env_ids) + + """ + Operations - Set pose. + """ + + def set_world_poses( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + env_ids: Sequence[int] | None = None, + convention: Literal["opengl", "ros", "world"] = "ros", + ): + r"""Set the pose of the camera w.r.t. the world frame using specified convention. + + Since different fields use different conventions for camera orientations, the method allows users to + set the camera poses in the specified convention. Possible conventions are: + + - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention + - :obj:`"ros"` - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention + - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention + + See :meth:`isaaclab.sensors.camera.utils.convert_camera_frame_orientation_convention` for more details + on the conventions. + + Args: + positions: The cartesian coordinates (in meters). Shape is (N, 3). + Defaults to None, in which case the camera position in not changed. + orientations: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the camera orientation in not changed. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + convention: The convention in which the poses are fed. Defaults to "ros". + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # convert to backend tensor + if positions is not None: + if isinstance(positions, np.ndarray): + positions = torch.from_numpy(positions).to(device=self._device) + elif not isinstance(positions, torch.Tensor): + positions = torch.tensor(positions, device=self._device) + # convert rotation matrix from input convention to OpenGL + if orientations is not None: + if isinstance(orientations, np.ndarray): + orientations = torch.from_numpy(orientations).to(device=self._device) + elif not isinstance(orientations, torch.Tensor): + orientations = torch.tensor(orientations, device=self._device) + orientations = convert_camera_frame_orientation_convention(orientations, origin=convention, target="opengl") + # set the pose + self._view.set_world_poses(positions, orientations, env_ids) + + def set_world_poses_from_view( + self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None + ): + """Set the poses of the camera from the eye position and look-at target position. + + Args: + eyes: The positions of the camera's eye. Shape is (N, 3). + targets: The target locations to look at. Shape is (N, 3). + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + NotImplementedError: If the stage up-axis is not "Y" or "Z". + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # get up axis of current stage + up_axis = stage_utils.get_stage_up_axis() + # set camera poses using the view + orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device)) + self._view.set_world_poses(eyes, orientations, env_ids) + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + if not self._is_initialized: + raise RuntimeError( + "Camera could not be initialized. Please ensure --enable_cameras is used to enable rendering." + ) + # reset the timestamps + super().reset(env_ids) + # resolve None + # note: cannot do smart indexing here since we do a for loop over data. + if env_ids is None: + env_ids = self._ALL_INDICES + # reset the data + # note: this recomputation is useful if one performs events such as randomizations on the camera poses. + self._update_poses(env_ids) + # Reset the frame count + self._frame[env_ids] = 0 + + """ + Implementation. + """ + + def _initialize_impl(self): + """Initializes the sensor handles and internal buffers. + + This function creates handles and registers the provided data types with the replicator registry to + be able to access the data from the sensor. It also initializes the internal buffers to store the data. + + Raises: + RuntimeError: If the number of camera prims in the view does not match the number of environments. + RuntimeError: If replicator was not found. + """ + carb_settings_iface = carb.settings.get_settings() + if not carb_settings_iface.get("/isaaclab/cameras_enabled"): + raise RuntimeError( + "A camera was spawned without the --enable_cameras flag. Please use --enable_cameras to enable" + " rendering." + ) + + import omni.replicator.core as rep + from omni.syntheticdata.scripts.SyntheticData import SyntheticData + + # Initialize parent class + super()._initialize_impl() + # Create a view for the sensor + self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + self._view.initialize() + # Check that sizes are correct + if self._view.count != self._num_envs: + raise RuntimeError( + f"Number of camera prims in the view ({self._view.count}) does not match" + f" the number of environments ({self._num_envs})." + ) + + # Create all env_ids buffer + self._ALL_INDICES = torch.arange(self._view.count, device=self._device, dtype=torch.long) + # Create frame count buffer + self._frame = torch.zeros(self._view.count, device=self._device, dtype=torch.long) + + # Attach the sensor data types to render node + self._render_product_paths: list[str] = list() + self._rep_registry: dict[str, list[rep.annotators.Annotator]] = {name: list() for name in self.cfg.data_types} + + # Obtain current stage + stage = omni.usd.get_context().get_stage() + # Convert all encapsulated prims to Camera + for cam_prim_path in self._view.prim_paths: + # Get camera prim + cam_prim = stage.GetPrimAtPath(cam_prim_path) + # Check if prim is a camera + if not cam_prim.IsA(UsdGeom.Camera): + raise RuntimeError(f"Prim at path '{cam_prim_path}' is not a Camera.") + # Add to list + sensor_prim = UsdGeom.Camera(cam_prim) + self._sensor_prims.append(sensor_prim) + + # Get render product + # From Isaac Sim 2023.1 onwards, render product is a HydraTexture so we need to extract the path + render_prod_path = rep.create.render_product(cam_prim_path, resolution=(self.cfg.width, self.cfg.height)) + if not isinstance(render_prod_path, str): + render_prod_path = render_prod_path.path + self._render_product_paths.append(render_prod_path) + + # Check if semantic types or semantic filter predicate is provided + if isinstance(self.cfg.semantic_filter, list): + semantic_filter_predicate = ":*; ".join(self.cfg.semantic_filter) + ":*" + elif isinstance(self.cfg.semantic_filter, str): + semantic_filter_predicate = self.cfg.semantic_filter + else: + raise ValueError(f"Semantic types must be a list or a string. Received: {self.cfg.semantic_filter}.") + # set the semantic filter predicate + # copied from rep.scripts.writes_default.basic_writer.py + SyntheticData.Get().set_instance_mapping_semantic_filter(semantic_filter_predicate) + + # Iterate over each data type and create annotator + # TODO: This will move out of the loop once Replicator supports multiple render products within a single + # annotator, i.e.: rep_annotator.attach(self._render_product_paths) + for name in self.cfg.data_types: + # note: we are verbose here to make it easier to understand the code. + # if colorize is true, the data is mapped to colors and a uint8 4 channel image is returned. + # if colorize is false, the data is returned as a uint32 image with ids as values. + if name == "semantic_segmentation": + init_params = { + "colorize": self.cfg.colorize_semantic_segmentation, + "mapping": json.dumps(self.cfg.semantic_segmentation_mapping), + } + elif name == "instance_segmentation_fast": + init_params = {"colorize": self.cfg.colorize_instance_segmentation} + elif name == "instance_id_segmentation_fast": + init_params = {"colorize": self.cfg.colorize_instance_id_segmentation} + else: + init_params = None + + # Resolve device name + if "cuda" in self._device: + device_name = self._device.split(":")[0] + else: + device_name = "cpu" + + # Map special cases to their corresponding annotator names + special_cases = {"rgba": "rgb", "depth": "distance_to_image_plane"} + # Get the annotator name, falling back to the original name if not a special case + annotator_name = special_cases.get(name, name) + # Create the annotator node + rep_annotator = rep.AnnotatorRegistry.get_annotator(annotator_name, init_params, device=device_name) + + # attach annotator to render product + rep_annotator.attach(render_prod_path) + # add to registry + self._rep_registry[name].append(rep_annotator) + + # Create internal buffers + self._create_buffers() + self._update_intrinsic_matrices(self._ALL_INDICES) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + # Increment frame count + self._frame[env_ids] += 1 + # -- pose + if self.cfg.update_latest_camera_pose: + self._update_poses(env_ids) + # -- read the data from annotator registry + # check if buffer is called for the first time. If so then, allocate the memory + if len(self._data.output) == 0: + # this is the first time buffer is called + # it allocates memory for all the sensors + self._create_annotator_data() + else: + # iterate over all the data types + for name, annotators in self._rep_registry.items(): + # iterate over all the annotators + for index in env_ids: + # get the output + output = annotators[index].get_data() + # process the output + data, info = self._process_annotator_output(name, output) + # add data to output + self._data.output[name][index] = data + # add info to output + self._data.info[index][name] = info + # NOTE: The `distance_to_camera` annotator returns the distance to the camera optical center. However, + # the replicator depth clipping is applied w.r.t. to the image plane which may result in values + # larger than the clipping range in the output. We apply an additional clipping to ensure values + # are within the clipping range for all the annotators. + if name == "distance_to_camera": + self._data.output[name][self._data.output[name] > self.cfg.spawn.clipping_range[1]] = torch.inf + # apply defined clipping behavior + if ( + name == "distance_to_camera" or name == "distance_to_image_plane" + ) and self.cfg.depth_clipping_behavior != "none": + self._data.output[name][torch.isinf(self._data.output[name])] = ( + 0.0 if self.cfg.depth_clipping_behavior == "zero" else self.cfg.spawn.clipping_range[1] + ) + + """ + Private Helpers + """ + + def _check_supported_data_types(self, cfg: CameraCfg): + """Checks if the data types are supported by the ray-caster camera.""" + # check if there is any intersection in unsupported types + # reason: these use np structured data types which we can't yet convert to torch tensor + common_elements = set(cfg.data_types) & Camera.UNSUPPORTED_TYPES + if common_elements: + # provide alternative fast counterparts + fast_common_elements = [] + for item in common_elements: + if "instance_segmentation" in item or "instance_id_segmentation" in item: + fast_common_elements.append(item + "_fast") + # raise error + raise ValueError( + f"Camera class does not support the following sensor types: {common_elements}." + "\n\tThis is because these sensor types output numpy structured data types which" + "can't be converted to torch tensors easily." + "\n\tHint: If you need to work with these sensor types, we recommend using their fast counterparts." + f"\n\t\tFast counterparts: {fast_common_elements}" + ) + + def _create_buffers(self): + """Create buffers for storing data.""" + # create the data object + # -- pose of the cameras + self._data.pos_w = torch.zeros((self._view.count, 3), device=self._device) + self._data.quat_w_world = torch.zeros((self._view.count, 4), device=self._device) + # -- intrinsic matrix + self._data.intrinsic_matrices = torch.zeros((self._view.count, 3, 3), device=self._device) + self._data.image_shape = self.image_shape + # -- output data + # lazy allocation of data dictionary + # since the size of the output data is not known in advance, we leave it as None + # the memory will be allocated when the buffer() function is called for the first time. + self._data.output = {} + self._data.info = [{name: None for name in self.cfg.data_types} for _ in range(self._view.count)] + + def _update_intrinsic_matrices(self, env_ids: Sequence[int]): + """Compute camera's matrix of intrinsic parameters. + + Also called calibration matrix. This matrix works for linear depth images. We assume square pixels. + + Note: + The calibration matrix projects points in the 3D scene onto an imaginary screen of the camera. + The coordinates of points on the image plane are in the homogeneous representation. + """ + # iterate over all cameras + for i in env_ids: + # Get corresponding sensor prim + sensor_prim = self._sensor_prims[i] + # get camera parameters + # currently rendering does not use aperture offsets or vertical aperture + focal_length = sensor_prim.GetFocalLengthAttr().Get() + horiz_aperture = sensor_prim.GetHorizontalApertureAttr().Get() + + # get viewport parameters + height, width = self.image_shape + # extract intrinsic parameters + f_x = (width * focal_length) / horiz_aperture + f_y = f_x + c_x = width * 0.5 + c_y = height * 0.5 + # create intrinsic matrix for depth linear + self._data.intrinsic_matrices[i, 0, 0] = f_x + self._data.intrinsic_matrices[i, 0, 2] = c_x + self._data.intrinsic_matrices[i, 1, 1] = f_y + self._data.intrinsic_matrices[i, 1, 2] = c_y + self._data.intrinsic_matrices[i, 2, 2] = 1 + + def _update_poses(self, env_ids: Sequence[int]): + """Computes the pose of the camera in the world frame with ROS convention. + + This methods uses the ROS convention to resolve the input pose. In this convention, + we assume that the camera front-axis is +Z-axis and up-axis is -Y-axis. + + Returns: + A tuple of the position (in meters) and quaternion (w, x, y, z). + """ + # check camera prim exists + if len(self._sensor_prims) == 0: + raise RuntimeError("Camera prim is None. Please call 'sim.play()' first.") + + # get the poses from the view + poses, quat = self._view.get_world_poses(env_ids) + self._data.pos_w[env_ids] = poses + self._data.quat_w_world[env_ids] = convert_camera_frame_orientation_convention( + quat, origin="opengl", target="world" + ) + + def _create_annotator_data(self): + """Create the buffers to store the annotator data. + + We create a buffer for each annotator and store the data in a dictionary. Since the data + shape is not known beforehand, we create a list of buffers and concatenate them later. + + This is an expensive operation and should be called only once. + """ + # add data from the annotators + for name, annotators in self._rep_registry.items(): + # create a list to store the data for each annotator + data_all_cameras = list() + # iterate over all the annotators + for index in self._ALL_INDICES: + # get the output + output = annotators[index].get_data() + # process the output + data, info = self._process_annotator_output(name, output) + # append the data + data_all_cameras.append(data) + # store the info + self._data.info[index][name] = info + # concatenate the data along the batch dimension + self._data.output[name] = torch.stack(data_all_cameras, dim=0) + # NOTE: `distance_to_camera` and `distance_to_image_plane` are not both clipped to the maximum defined + # in the clipping range. The clipping is applied only to `distance_to_image_plane` and then both + # outputs are only clipped where the values in `distance_to_image_plane` exceed the threshold. To + # have a unified behavior between all cameras, we clip both outputs to the maximum value defined. + if name == "distance_to_camera": + self._data.output[name][self._data.output[name] > self.cfg.spawn.clipping_range[1]] = torch.inf + # clip the data if needed + if ( + name == "distance_to_camera" or name == "distance_to_image_plane" + ) and self.cfg.depth_clipping_behavior != "none": + self._data.output[name][torch.isinf(self._data.output[name])] = ( + 0.0 if self.cfg.depth_clipping_behavior == "zero" else self.cfg.spawn.clipping_range[1] + ) + + def _process_annotator_output(self, name: str, output: Any) -> tuple[torch.tensor, dict | None]: + """Process the annotator output. + + This function is called after the data has been collected from all the cameras. + """ + # extract info and data from the output + if isinstance(output, dict): + data = output["data"] + info = output["info"] + else: + data = output + info = None + # convert data into torch tensor + data = convert_to_torch(data, device=self.device) + + # process data for different segmentation types + # Note: Replicator returns raw buffers of dtype int32 for segmentation types + # so we need to convert them to uint8 4 channel images for colorized types + height, width = self.image_shape + if name == "semantic_segmentation": + if self.cfg.colorize_semantic_segmentation: + data = data.view(torch.uint8).reshape(height, width, -1) + else: + data = data.view(height, width, 1) + elif name == "instance_segmentation_fast": + if self.cfg.colorize_instance_segmentation: + data = data.view(torch.uint8).reshape(height, width, -1) + else: + data = data.view(height, width, 1) + elif name == "instance_id_segmentation_fast": + if self.cfg.colorize_instance_id_segmentation: + data = data.view(torch.uint8).reshape(height, width, -1) + else: + data = data.view(height, width, 1) + # make sure buffer dimensions are consistent as (H, W, C) + elif name == "distance_to_camera" or name == "distance_to_image_plane" or name == "depth": + data = data.view(height, width, 1) + # we only return the RGB channels from the RGBA output if rgb is required + # normals return (x, y, z) in first 3 channels, 4th channel is unused + elif name == "rgb" or name == "normals": + data = data[..., :3] + # motion vectors return (x, y) in first 2 channels, 3rd and 4th channels are unused + elif name == "motion_vectors": + data = data[..., :2] + + # return the data and info + return data, info + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_cfg.py new file mode 100644 index 00000000000..1988a13f0fe --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_cfg.py @@ -0,0 +1,147 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim import FisheyeCameraCfg, PinholeCameraCfg +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg +from .camera import Camera + + +@configclass +class CameraCfg(SensorBaseCfg): + """Configuration for a camera sensor.""" + + @configclass + class OffsetCfg: + """The offset pose of the sensor's frame from the sensor's parent frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + convention: Literal["opengl", "ros", "world"] = "ros" + """The convention in which the frame offset is applied. Defaults to "ros". + + - ``"opengl"`` - forward axis: ``-Z`` - up axis: ``+Y`` - Offset is applied in the OpenGL (Usd.Camera) convention. + - ``"ros"`` - forward axis: ``+Z`` - up axis: ``-Y`` - Offset is applied in the ROS convention. + - ``"world"`` - forward axis: ``+X`` - up axis: ``+Z`` - Offset is applied in the World Frame convention. + + """ + + class_type: type = Camera + + offset: OffsetCfg = OffsetCfg() + """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity. + + Note: + The parent frame is the frame the sensor attaches to. For example, the parent frame of a + camera at path ``/World/envs/env_0/Robot/Camera`` is ``/World/envs/env_0/Robot``. + """ + + spawn: PinholeCameraCfg | FisheyeCameraCfg | None = MISSING + """Spawn configuration for the asset. + + If None, then the prim is not spawned by the asset. Instead, it is assumed that the + asset is already present in the scene. + """ + + depth_clipping_behavior: Literal["max", "zero", "none"] = "none" + """Clipping behavior for the camera for values exceed the maximum value. Defaults to "none". + + - ``"max"``: Values are clipped to the maximum value. + - ``"zero"``: Values are clipped to zero. + - ``"none``: No clipping is applied. Values will be returned as ``inf``. + """ + + data_types: list[str] = ["rgb"] + """List of sensor names/types to enable for the camera. Defaults to ["rgb"]. + + Please refer to the :class:`Camera` class for a list of available data types. + """ + + width: int = MISSING + """Width of the image in pixels.""" + + height: int = MISSING + """Height of the image in pixels.""" + + update_latest_camera_pose: bool = False + """Whether to update the latest camera pose when fetching the camera's data. Defaults to False. + + If True, the latest camera pose is updated in the camera's data which will slow down performance + due to the use of :class:`XformPrimView`. + If False, the pose of the camera during initialization is returned. + """ + + semantic_filter: str | list[str] = "*:*" + """A string or a list specifying a semantic filter predicate. Defaults to ``"*:*"``. + + If a string, it should be a disjunctive normal form of (semantic type, labels). For examples: + + * ``"typeA : labelA & !labelB | labelC , typeB: labelA ; typeC: labelE"``: + All prims with semantic type "typeA" and label "labelA" but not "labelB" or with label "labelC". + Also, all prims with semantic type "typeB" and label "labelA", or with semantic type "typeC" and label "labelE". + * ``"typeA : * ; * : labelA"``: All prims with semantic type "typeA" or with label "labelA" + + If a list of strings, each string should be a semantic type. The segmentation for prims with + semantics of the specified types will be retrieved. For example, if the list is ["class"], only + the segmentation for prims with semantics of type "class" will be retrieved. + + .. seealso:: + + For more information on the semantics filter, see the documentation on `Replicator Semantics Schema Editor`_. + + .. _Replicator Semantics Schema Editor: https://docs.omniverse.nvidia.com/extensions/latest/ext_replicator/semantics_schema_editor.html#semantics-filtering + """ + + colorize_semantic_segmentation: bool = True + """Whether to colorize the semantic segmentation images. Defaults to True. + + If True, semantic segmentation is converted to an image where semantic IDs are mapped to colors + and returned as a ``uint8`` 4-channel array. If False, the output is returned as a ``int32`` array. + """ + + colorize_instance_id_segmentation: bool = True + """Whether to colorize the instance ID segmentation images. Defaults to True. + + If True, instance id segmentation is converted to an image where instance IDs are mapped to colors. + and returned as a ``uint8`` 4-channel array. If False, the output is returned as a ``int32`` array. + """ + + colorize_instance_segmentation: bool = True + """Whether to colorize the instance ID segmentation images. Defaults to True. + + If True, instance segmentation is converted to an image where instance IDs are mapped to colors. + and returned as a ``uint8`` 4-channel array. If False, the output is returned as a ``int32`` array. + """ + + semantic_segmentation_mapping: dict = {} + """Dictionary mapping semantics to specific colours + + Eg. + + .. code-block:: python + + { + "class:cube_1": (255, 36, 66, 255), + "class:cube_2": (255, 184, 48, 255), + "class:cube_3": (55, 255, 139, 255), + "class:table": (255, 237, 218, 255), + "class:ground": (100, 100, 100, 255), + "class:robot": (61, 178, 255, 255), + } + + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_data.py new file mode 100644 index 00000000000..58e5d7b96db --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/camera_data.py @@ -0,0 +1,96 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass +from typing import Any + +from isaaclab.utils.math import convert_camera_frame_orientation_convention + + +@dataclass +class CameraData: + """Data container for the camera sensor.""" + + ## + # Frame state. + ## + + pos_w: torch.Tensor = None + """Position of the sensor origin in world frame, following ROS convention. + + Shape is (N, 3) where N is the number of sensors. + """ + + quat_w_world: torch.Tensor = None + """Quaternion orientation `(w, x, y, z)` of the sensor origin in world frame, following the world coordinate frame + + .. note:: + World frame convention follows the camera aligned with forward axis +X and up axis +Z. + + Shape is (N, 4) where N is the number of sensors. + """ + + ## + # Camera data + ## + + image_shape: tuple[int, int] = None + """A tuple containing (height, width) of the camera sensor.""" + + intrinsic_matrices: torch.Tensor = None + """The intrinsic matrices for the camera. + + Shape is (N, 3, 3) where N is the number of sensors. + """ + + output: dict[str, torch.Tensor] = None + """The retrieved sensor data with sensor types as key. + + The format of the data is available in the `Replicator Documentation`_. For semantic-based data, + this corresponds to the ``"data"`` key in the output of the sensor. + + .. _Replicator Documentation: https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_replicator/annotators_details.html#annotator-output + """ + + info: list[dict[str, Any]] = None + """The retrieved sensor info with sensor types as key. + + This contains extra information provided by the sensor such as semantic segmentation label mapping, prim paths. + For semantic-based data, this corresponds to the ``"info"`` key in the output of the sensor. For other sensor + types, the info is empty. + """ + + ## + # Additional Frame orientation conventions + ## + + @property + def quat_w_ros(self) -> torch.Tensor: + """Quaternion orientation `(w, x, y, z)` of the sensor origin in the world frame, following ROS convention. + + .. note:: + ROS convention follows the camera aligned with forward axis +Z and up axis -Y. + + Shape is (N, 4) where N is the number of sensors. + """ + return convert_camera_frame_orientation_convention(self.quat_w_world, origin="world", target="ros") + + @property + def quat_w_opengl(self) -> torch.Tensor: + """Quaternion orientation `(w, x, y, z)` of the sensor origin in the world frame, following + Opengl / USD Camera convention. + + .. note:: + OpenGL convention follows the camera aligned with forward axis -Z and up axis +Y. + + Shape is (N, 4) where N is the number of sensors. + """ + return convert_camera_frame_orientation_convention(self.quat_w_world, origin="world", target="opengl") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera.py new file mode 100644 index 00000000000..3c3e4cebb6c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera.py @@ -0,0 +1,435 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import json +import math +import numpy as np +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import carb +import omni.usd +import warp as wp +from isaacsim.core.prims import XFormPrim +from isaacsim.core.version import get_version +from pxr import UsdGeom + +from isaaclab.utils.warp.kernels import reshape_tiled_image + +from ..sensor_base import SensorBase +from .camera import Camera + +if TYPE_CHECKING: + from .tiled_camera_cfg import TiledCameraCfg + + +class TiledCamera(Camera): + r"""The tiled rendering based camera sensor for acquiring the same data as the Camera class. + + This class inherits from the :class:`Camera` class but uses the tiled-rendering API to acquire + the visual data. Tiled-rendering concatenates the rendered images from multiple cameras into a single image. + This allows for rendering multiple cameras in parallel and is useful for rendering large scenes with multiple + cameras efficiently. + + The following sensor types are supported: + + - ``"rgb"``: A 3-channel rendered color image. + - ``"rgba"``: A 4-channel rendered color image with alpha channel. + - ``"distance_to_camera"``: An image containing the distance to camera optical center. + - ``"distance_to_image_plane"``: An image containing distances of 3D points from camera plane along camera's z-axis. + - ``"depth"``: Alias for ``"distance_to_image_plane"``. + - ``"normals"``: An image containing the local surface normal vectors at each pixel. + - ``"motion_vectors"``: An image containing the motion vector data at each pixel. + - ``"semantic_segmentation"``: The semantic segmentation data. + - ``"instance_segmentation_fast"``: The instance segmentation data. + - ``"instance_id_segmentation_fast"``: The instance id segmentation data. + + .. note:: + Currently the following sensor types are not supported in a "view" format: + + - ``"instance_segmentation"``: The instance segmentation data. Please use the fast counterparts instead. + - ``"instance_id_segmentation"``: The instance id segmentation data. Please use the fast counterparts instead. + - ``"bounding_box_2d_tight"``: The tight 2D bounding box data (only contains non-occluded regions). + - ``"bounding_box_2d_tight_fast"``: The tight 2D bounding box data (only contains non-occluded regions). + - ``"bounding_box_2d_loose"``: The loose 2D bounding box data (contains occluded regions). + - ``"bounding_box_2d_loose_fast"``: The loose 2D bounding box data (contains occluded regions). + - ``"bounding_box_3d"``: The 3D view space bounding box data. + - ``"bounding_box_3d_fast"``: The 3D view space bounding box data. + + .. _replicator extension: https://docs.omniverse.nvidia.com/extensions/latest/ext_replicator/annotators_details.html#annotator-output + .. _USDGeom Camera: https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html + + .. versionadded:: v1.0.0 + + This feature is available starting from Isaac Sim 4.2. Before this version, the tiled rendering APIs + were not available. + + """ + + cfg: TiledCameraCfg + """The configuration parameters.""" + + def __init__(self, cfg: TiledCameraCfg): + """Initializes the tiled camera sensor. + + Args: + cfg: The configuration parameters. + + Raises: + RuntimeError: If no camera prim is found at the given path. + RuntimeError: If Isaac Sim version < 4.2 + ValueError: If the provided data types are not supported by the camera. + """ + isaac_sim_version = float(".".join(get_version()[2:4])) + if isaac_sim_version < 4.2: + raise RuntimeError( + f"TiledCamera is only available from Isaac Sim 4.2.0. Current version is {isaac_sim_version}. Please" + " update to Isaac Sim 4.2.0" + ) + super().__init__(cfg) + + def __del__(self): + """Unsubscribes from callbacks and detach from the replicator registry.""" + # unsubscribe from callbacks + SensorBase.__del__(self) + # detach from the replicator registry + for annotator in self._annotators.values(): + annotator.detach(self.render_product_paths) + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + # message for class + return ( + f"Tiled Camera @ '{self.cfg.prim_path}': \n" + f"\tdata types : {list(self.data.output.keys())} \n" + f"\tsemantic filter : {self.cfg.semantic_filter}\n" + f"\tcolorize semantic segm. : {self.cfg.colorize_semantic_segmentation}\n" + f"\tcolorize instance segm. : {self.cfg.colorize_instance_segmentation}\n" + f"\tcolorize instance id segm.: {self.cfg.colorize_instance_id_segmentation}\n" + f"\tupdate period (s): {self.cfg.update_period}\n" + f"\tshape : {self.image_shape}\n" + f"\tnumber of sensors : {self._view.count}" + ) + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + if not self._is_initialized: + raise RuntimeError( + "TiledCamera could not be initialized. Please ensure --enable_cameras is used to enable rendering." + ) + # reset the timestamps + SensorBase.reset(self, env_ids) + # resolve None + if env_ids is None: + env_ids = slice(None) + # reset the frame count + self._frame[env_ids] = 0 + + """ + Implementation. + """ + + def _initialize_impl(self): + """Initializes the sensor handles and internal buffers. + + This function creates handles and registers the provided data types with the replicator registry to + be able to access the data from the sensor. It also initializes the internal buffers to store the data. + + Raises: + RuntimeError: If the number of camera prims in the view does not match the number of environments. + RuntimeError: If replicator was not found. + """ + carb_settings_iface = carb.settings.get_settings() + if not carb_settings_iface.get("/isaaclab/cameras_enabled"): + raise RuntimeError( + "A camera was spawned without the --enable_cameras flag. Please use --enable_cameras to enable" + " rendering." + ) + + import omni.replicator.core as rep + + # Initialize parent class + SensorBase._initialize_impl(self) + # Create a view for the sensor + self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + self._view.initialize() + # Check that sizes are correct + if self._view.count != self._num_envs: + raise RuntimeError( + f"Number of camera prims in the view ({self._view.count}) does not match" + f" the number of environments ({self._num_envs})." + ) + + # Create all env_ids buffer + self._ALL_INDICES = torch.arange(self._view.count, device=self._device, dtype=torch.long) + # Create frame count buffer + self._frame = torch.zeros(self._view.count, device=self._device, dtype=torch.long) + + # Obtain current stage + stage = omni.usd.get_context().get_stage() + # Convert all encapsulated prims to Camera + for cam_prim_path in self._view.prim_paths: + # Get camera prim + cam_prim = stage.GetPrimAtPath(cam_prim_path) + # Check if prim is a camera + if not cam_prim.IsA(UsdGeom.Camera): + raise RuntimeError(f"Prim at path '{cam_prim_path}' is not a Camera.") + # Add to list + sensor_prim = UsdGeom.Camera(cam_prim) + self._sensor_prims.append(sensor_prim) + + # Create replicator tiled render product + rp = rep.create.render_product_tiled( + cameras=self._view.prim_paths, tile_resolution=(self.cfg.width, self.cfg.height) + ) + self._render_product_paths = [rp.path] + + # Define the annotators based on requested data types + self._annotators = dict() + for annotator_type in self.cfg.data_types: + if annotator_type == "rgba" or annotator_type == "rgb": + annotator = rep.AnnotatorRegistry.get_annotator("rgb", device=self.device, do_array_copy=False) + self._annotators["rgba"] = annotator + elif annotator_type == "depth" or annotator_type == "distance_to_image_plane": + # keep depth for backwards compatibility + annotator = rep.AnnotatorRegistry.get_annotator( + "distance_to_image_plane", device=self.device, do_array_copy=False + ) + self._annotators[annotator_type] = annotator + # note: we are verbose here to make it easier to understand the code. + # if colorize is true, the data is mapped to colors and a uint8 4 channel image is returned. + # if colorize is false, the data is returned as a uint32 image with ids as values. + else: + init_params = None + if annotator_type == "semantic_segmentation": + init_params = { + "colorize": self.cfg.colorize_semantic_segmentation, + "mapping": json.dumps(self.cfg.semantic_segmentation_mapping), + } + elif annotator_type == "instance_segmentation_fast": + init_params = {"colorize": self.cfg.colorize_instance_segmentation} + elif annotator_type == "instance_id_segmentation_fast": + init_params = {"colorize": self.cfg.colorize_instance_id_segmentation} + + annotator = rep.AnnotatorRegistry.get_annotator( + annotator_type, init_params, device=self.device, do_array_copy=False + ) + self._annotators[annotator_type] = annotator + + # Attach the annotator to the render product + for annotator in self._annotators.values(): + annotator.attach(self._render_product_paths) + + # Create internal buffers + self._create_buffers() + + def _update_buffers_impl(self, env_ids: Sequence[int]): + # Increment frame count + self._frame[env_ids] += 1 + + # update latest camera pose + if self.cfg.update_latest_camera_pose: + self._update_poses(env_ids) + + # Extract the flattened image buffer + for data_type, annotator in self._annotators.items(): + # check whether returned data is a dict (used for segmentation) + output = annotator.get_data() + if isinstance(output, dict): + tiled_data_buffer = output["data"] + self._data.info[data_type] = output["info"] + else: + tiled_data_buffer = output + + # convert data buffer to warp array + if isinstance(tiled_data_buffer, np.ndarray): + tiled_data_buffer = wp.array(tiled_data_buffer, device=self.device, dtype=wp.uint8) + else: + tiled_data_buffer = tiled_data_buffer.to(device=self.device) + + # process data for different segmentation types + # Note: Replicator returns raw buffers of dtype uint32 for segmentation types + # so we need to convert them to uint8 4 channel images for colorized types + if ( + (data_type == "semantic_segmentation" and self.cfg.colorize_semantic_segmentation) + or (data_type == "instance_segmentation_fast" and self.cfg.colorize_instance_segmentation) + or (data_type == "instance_id_segmentation_fast" and self.cfg.colorize_instance_id_segmentation) + ): + tiled_data_buffer = wp.array( + ptr=tiled_data_buffer.ptr, shape=(*tiled_data_buffer.shape, 4), dtype=wp.uint8, device=self.device + ) + + # For motion vectors, we only require the first two channels of the tiled buffer + # Note: Not doing this breaks the alignment of the data (check: https://github.com/isaac-sim/IsaacLab/issues/2003) + if data_type == "motion_vectors": + tiled_data_buffer = tiled_data_buffer[:, :, :2].contiguous() + + wp.launch( + kernel=reshape_tiled_image, + dim=(self._view.count, self.cfg.height, self.cfg.width), + inputs=[ + tiled_data_buffer.flatten(), + wp.from_torch(self._data.output[data_type]), # zero-copy alias + *list(self._data.output[data_type].shape[1:]), # height, width, num_channels + self._tiling_grid_shape()[0], # num_tiles_x + ], + device=self.device, + ) + + # alias rgb as first 3 channels of rgba + if data_type == "rgba" and "rgb" in self.cfg.data_types: + self._data.output["rgb"] = self._data.output["rgba"][..., :3] + + # NOTE: The `distance_to_camera` annotator returns the distance to the camera optical center. However, + # the replicator depth clipping is applied w.r.t. to the image plane which may result in values + # larger than the clipping range in the output. We apply an additional clipping to ensure values + # are within the clipping range for all the annotators. + if data_type == "distance_to_camera": + self._data.output[data_type][ + self._data.output[data_type] > self.cfg.spawn.clipping_range[1] + ] = torch.inf + # apply defined clipping behavior + if ( + data_type == "distance_to_camera" or data_type == "distance_to_image_plane" or data_type == "depth" + ) and self.cfg.depth_clipping_behavior != "none": + self._data.output[data_type][torch.isinf(self._data.output[data_type])] = ( + 0.0 if self.cfg.depth_clipping_behavior == "zero" else self.cfg.spawn.clipping_range[1] + ) + + """ + Private Helpers + """ + + def _check_supported_data_types(self, cfg: TiledCameraCfg): + """Checks if the data types are supported by the ray-caster camera.""" + # check if there is any intersection in unsupported types + # reason: these use np structured data types which we can't yet convert to torch tensor + common_elements = set(cfg.data_types) & Camera.UNSUPPORTED_TYPES + if common_elements: + # provide alternative fast counterparts + fast_common_elements = [] + for item in common_elements: + if "instance_segmentation" in item or "instance_id_segmentation" in item: + fast_common_elements.append(item + "_fast") + # raise error + raise ValueError( + f"TiledCamera class does not support the following sensor types: {common_elements}." + "\n\tThis is because these sensor types output numpy structured data types which" + "can't be converted to torch tensors easily." + "\n\tHint: If you need to work with these sensor types, we recommend using their fast counterparts." + f"\n\t\tFast counterparts: {fast_common_elements}" + ) + + def _create_buffers(self): + """Create buffers for storing data.""" + # create the data object + # -- pose of the cameras + self._data.pos_w = torch.zeros((self._view.count, 3), device=self._device) + self._data.quat_w_world = torch.zeros((self._view.count, 4), device=self._device) + self._update_poses(self._ALL_INDICES) + # -- intrinsic matrix + self._data.intrinsic_matrices = torch.zeros((self._view.count, 3, 3), device=self._device) + self._update_intrinsic_matrices(self._ALL_INDICES) + self._data.image_shape = self.image_shape + # -- output data + data_dict = dict() + if "rgba" in self.cfg.data_types or "rgb" in self.cfg.data_types: + data_dict["rgba"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 4), device=self.device, dtype=torch.uint8 + ).contiguous() + if "rgb" in self.cfg.data_types: + # RGB is the first 3 channels of RGBA + data_dict["rgb"] = data_dict["rgba"][..., :3] + if "distance_to_image_plane" in self.cfg.data_types: + data_dict["distance_to_image_plane"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.float32 + ).contiguous() + if "depth" in self.cfg.data_types: + data_dict["depth"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.float32 + ).contiguous() + if "distance_to_camera" in self.cfg.data_types: + data_dict["distance_to_camera"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.float32 + ).contiguous() + if "normals" in self.cfg.data_types: + data_dict["normals"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 3), device=self.device, dtype=torch.float32 + ).contiguous() + if "motion_vectors" in self.cfg.data_types: + data_dict["motion_vectors"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 2), device=self.device, dtype=torch.float32 + ).contiguous() + if "semantic_segmentation" in self.cfg.data_types: + if self.cfg.colorize_semantic_segmentation: + data_dict["semantic_segmentation"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 4), device=self.device, dtype=torch.uint8 + ).contiguous() + else: + data_dict["semantic_segmentation"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.int32 + ).contiguous() + if "instance_segmentation_fast" in self.cfg.data_types: + if self.cfg.colorize_instance_segmentation: + data_dict["instance_segmentation_fast"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 4), device=self.device, dtype=torch.uint8 + ).contiguous() + else: + data_dict["instance_segmentation_fast"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.int32 + ).contiguous() + if "instance_id_segmentation_fast" in self.cfg.data_types: + if self.cfg.colorize_instance_id_segmentation: + data_dict["instance_id_segmentation_fast"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 4), device=self.device, dtype=torch.uint8 + ).contiguous() + else: + data_dict["instance_id_segmentation_fast"] = torch.zeros( + (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device, dtype=torch.int32 + ).contiguous() + + self._data.output = data_dict + self._data.info = dict() + + def _tiled_image_shape(self) -> tuple[int, int]: + """Returns a tuple containing the dimension of the tiled image.""" + cols, rows = self._tiling_grid_shape() + return (self.cfg.width * cols, self.cfg.height * rows) + + def _tiling_grid_shape(self) -> tuple[int, int]: + """Returns a tuple containing the tiling grid dimension.""" + cols = math.ceil(math.sqrt(self._view.count)) + rows = math.ceil(self._view.count / cols) + return (cols, rows) + + def _create_annotator_data(self): + # we do not need to create annotator data for the tiled camera sensor + raise RuntimeError("This function should not be called for the tiled camera sensor.") + + def _process_annotator_output(self, name: str, output: Any) -> tuple[torch.tensor, dict | None]: + # we do not need to process annotator output for the tiled camera sensor + raise RuntimeError("This function should not be called for the tiled camera sensor.") + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera_cfg.py new file mode 100644 index 00000000000..3f485b74beb --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/tiled_camera_cfg.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .camera_cfg import CameraCfg +from .tiled_camera import TiledCamera + + +@configclass +class TiledCameraCfg(CameraCfg): + """Configuration for a tiled rendering-based camera sensor.""" + + class_type: type = TiledCamera diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/utils.py new file mode 100644 index 00000000000..2d2bdc77aec --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/camera/utils.py @@ -0,0 +1,276 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Helper functions to project between pointcloud and depth images.""" + +# needed to import for allowing type-hinting: torch.device | str | None +from __future__ import annotations + +import numpy as np +import torch +from collections.abc import Sequence + +import warp as wp + +import isaaclab.utils.math as math_utils +from isaaclab.utils.array import TensorData, convert_to_torch + +""" +Depth <-> Pointcloud conversions. +""" + + +def transform_points( + points: TensorData, + position: Sequence[float] | None = None, + orientation: Sequence[float] | None = None, + device: torch.device | str | None = None, +) -> np.ndarray | torch.Tensor: + r"""Transform input points in a given frame to a target frame. + + This function transform points from a source frame to a target frame. The transformation is defined by the + position ``t`` and orientation ``R`` of the target frame in the source frame. + + .. math:: + p_{target} = R_{target} \times p_{source} + t_{target} + + If either the inputs `position` and `orientation` are None, the corresponding transformation is not applied. + + Args: + points: a tensor of shape (p, 3) or (n, p, 3) comprising of 3d points in source frame. + position: The position of source frame in target frame. Defaults to None. + orientation: The orientation (w, x, y, z) of source frame in target frame. + Defaults to None. + device: The device for torch where the computation + should be executed. Defaults to None, i.e. takes the device that matches the depth image. + + Returns: + A tensor of shape (N, 3) comprising of 3D points in target frame. + If the input is a numpy array, the output is a numpy array. Otherwise, it is a torch tensor. + """ + # check if numpy + is_numpy = isinstance(points, np.ndarray) + # decide device + if device is None and is_numpy: + device = torch.device("cpu") + # convert to torch + points = convert_to_torch(points, dtype=torch.float32, device=device) + # update the device with the device of the depth image + # note: this is needed since warp does not provide the device directly + device = points.device + # apply rotation + if orientation is not None: + orientation = convert_to_torch(orientation, dtype=torch.float32, device=device) + # apply translation + if position is not None: + position = convert_to_torch(position, dtype=torch.float32, device=device) + # apply transformation + points = math_utils.transform_points(points, position, orientation) + + # return everything according to input type + if is_numpy: + return points.detach().cpu().numpy() + else: + return points + + +def create_pointcloud_from_depth( + intrinsic_matrix: np.ndarray | torch.Tensor | wp.array, + depth: np.ndarray | torch.Tensor | wp.array, + keep_invalid: bool = False, + position: Sequence[float] | None = None, + orientation: Sequence[float] | None = None, + device: torch.device | str | None = None, +) -> np.ndarray | torch.Tensor: + r"""Creates pointcloud from input depth image and camera intrinsic matrix. + + This function creates a pointcloud from a depth image and camera intrinsic matrix. The pointcloud is + computed using the following equation: + + .. math:: + p_{camera} = K^{-1} \times [u, v, 1]^T \times d + + where :math:`K` is the camera intrinsic matrix, :math:`u` and :math:`v` are the pixel coordinates and + :math:`d` is the depth value at the pixel. + + Additionally, the pointcloud can be transformed from the camera frame to a target frame by providing + the position ``t`` and orientation ``R`` of the camera in the target frame: + + .. math:: + p_{target} = R_{target} \times p_{camera} + t_{target} + + Args: + intrinsic_matrix: A (3, 3) array providing camera's calibration matrix. + depth: An array of shape (H, W) with values encoding the depth measurement. + keep_invalid: Whether to keep invalid points in the cloud or not. Invalid points + correspond to pixels with depth values 0.0 or NaN. Defaults to False. + position: The position of the camera in a target frame. Defaults to None. + orientation: The orientation (w, x, y, z) of the camera in a target frame. Defaults to None. + device: The device for torch where the computation should be executed. + Defaults to None, i.e. takes the device that matches the depth image. + + Returns: + An array/tensor of shape (N, 3) comprising of 3D coordinates of points. + The returned datatype is torch if input depth is of type torch.tensor or wp.array. Otherwise, a np.ndarray + is returned. + """ + # We use PyTorch here for matrix multiplication since it is compiled with Intel MKL while numpy + # by default uses OpenBLAS. With PyTorch (CPU), we could process a depth image of size (480, 640) + # in 0.0051 secs, while with numpy it took 0.0292 secs. + + # convert to numpy matrix + is_numpy = isinstance(depth, np.ndarray) + # decide device + if device is None and is_numpy: + device = torch.device("cpu") + # convert depth to torch tensor + depth = convert_to_torch(depth, dtype=torch.float32, device=device) + # update the device with the device of the depth image + # note: this is needed since warp does not provide the device directly + device = depth.device + # convert inputs to torch tensors + intrinsic_matrix = convert_to_torch(intrinsic_matrix, dtype=torch.float32, device=device) + if position is not None: + position = convert_to_torch(position, dtype=torch.float32, device=device) + if orientation is not None: + orientation = convert_to_torch(orientation, dtype=torch.float32, device=device) + # compute pointcloud + depth_cloud = math_utils.unproject_depth(depth, intrinsic_matrix) + # convert 3D points to world frame + depth_cloud = math_utils.transform_points(depth_cloud, position, orientation) + + # keep only valid entries if flag is set + if not keep_invalid: + pts_idx_to_keep = torch.all(torch.logical_and(~torch.isnan(depth_cloud), ~torch.isinf(depth_cloud)), dim=1) + depth_cloud = depth_cloud[pts_idx_to_keep, ...] + + # return everything according to input type + if is_numpy: + return depth_cloud.detach().cpu().numpy() + else: + return depth_cloud + + +def create_pointcloud_from_rgbd( + intrinsic_matrix: torch.Tensor | np.ndarray | wp.array, + depth: torch.Tensor | np.ndarray | wp.array, + rgb: torch.Tensor | wp.array | np.ndarray | tuple[float, float, float] = None, + normalize_rgb: bool = False, + position: Sequence[float] | None = None, + orientation: Sequence[float] | None = None, + device: torch.device | str | None = None, + num_channels: int = 3, +) -> tuple[torch.Tensor, torch.Tensor] | tuple[np.ndarray, np.ndarray]: + """Creates pointcloud from input depth image and camera transformation matrix. + + This function provides the same functionality as :meth:`create_pointcloud_from_depth` but also allows + to provide the RGB values for each point. + + The ``rgb`` attribute is used to resolve the corresponding point's color: + + - If a ``np.array``/``wp.array``/``torch.tensor`` of shape (H, W, 3), then the corresponding channels encode RGB values. + - If a tuple, then the point cloud has a single color specified by the values (r, g, b). + - If None, then default color is white, i.e. (0, 0, 0). + + If the input ``normalize_rgb`` is set to :obj:`True`, then the RGB values are normalized to be in the range [0, 1]. + + Args: + intrinsic_matrix: A (3, 3) array/tensor providing camera's calibration matrix. + depth: An array/tensor of shape (H, W) with values encoding the depth measurement. + rgb: Color for generated point cloud. Defaults to None. + normalize_rgb: Whether to normalize input rgb. Defaults to False. + position: The position of the camera in a target frame. Defaults to None. + orientation: The orientation `(w, x, y, z)` of the camera in a target frame. Defaults to None. + device: The device for torch where the computation should be executed. Defaults to None, in which case + it takes the device that matches the depth image. + num_channels: Number of channels in RGB pointcloud. Defaults to 3. + + Returns: + A tuple of (N, 3) arrays or tensors containing the 3D coordinates of points and their RGB color respectively. + The returned datatype is torch if input depth is of type torch.tensor or wp.array. Otherwise, a np.ndarray + is returned. + + Raises: + ValueError: When rgb image is a numpy array but not of shape (H, W, 3) or (H, W, 4). + """ + # check valid inputs + if rgb is not None and not isinstance(rgb, tuple): + if len(rgb.shape) == 3: + if rgb.shape[2] not in [3, 4]: + raise ValueError(f"Input rgb image of invalid shape: {rgb.shape} != (H, W, 3) or (H, W, 4).") + else: + raise ValueError(f"Input rgb image not three-dimensional. Received shape: {rgb.shape}.") + if num_channels not in [3, 4]: + raise ValueError(f"Invalid number of channels: {num_channels} != 3 or 4.") + + # check if input depth is numpy array + is_numpy = isinstance(depth, np.ndarray) + # decide device + if device is None and is_numpy: + device = torch.device("cpu") + # convert depth to torch tensor + if is_numpy: + depth = torch.from_numpy(depth).to(device=device) + # retrieve XYZ pointcloud + points_xyz = create_pointcloud_from_depth(intrinsic_matrix, depth, True, position, orientation, device=device) + + # get image height and width + im_height, im_width = depth.shape[:2] + # total number of points + num_points = im_height * im_width + # extract color value + if rgb is not None: + if isinstance(rgb, (np.ndarray, torch.Tensor, wp.array)): + # copy numpy array to preserve + rgb = convert_to_torch(rgb, device=device, dtype=torch.float32) + rgb = rgb[:, :, :3] + # convert the matrix to (W, H, 3) from (H, W, 3) since depth processing + # is done in the order (u, v) where u: (0, W-1) and v: (0 - H-1) + points_rgb = rgb.permute(1, 0, 2).reshape(-1, 3) + elif isinstance(rgb, (tuple, list)): + # same color for all points + points_rgb = torch.Tensor((rgb,) * num_points, device=device, dtype=torch.uint8) + else: + # default color is white + points_rgb = torch.Tensor(((0, 0, 0),) * num_points, device=device, dtype=torch.uint8) + else: + points_rgb = torch.Tensor(((0, 0, 0),) * num_points, device=device, dtype=torch.uint8) + # normalize color values + if normalize_rgb: + points_rgb = points_rgb.float() / 255 + + # remove invalid points + pts_idx_to_keep = torch.all(torch.logical_and(~torch.isnan(points_xyz), ~torch.isinf(points_xyz)), dim=1) + points_rgb = points_rgb[pts_idx_to_keep, ...] + points_xyz = points_xyz[pts_idx_to_keep, ...] + + # add additional channels if required + if num_channels == 4: + points_rgb = torch.nn.functional.pad(points_rgb, (0, 1), mode="constant", value=1.0) + + # return everything according to input type + if is_numpy: + return points_xyz.cpu().numpy(), points_rgb.cpu().numpy() + else: + return points_xyz, points_rgb + + +def save_images_to_file(images: torch.Tensor, file_path: str): + """Save images to file. + + Args: + images: A tensor of shape (N, H, W, C) containing the images. + file_path: The path to save the images to. + """ + from torchvision.utils import make_grid, save_image + + save_image( + make_grid(torch.swapaxes(images.unsqueeze(1), 1, -1).squeeze(-1), nrow=round(images.shape[0] ** 0.5)), file_path + ) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/__init__.py new file mode 100644 index 00000000000..9c3aed12f7c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for rigid contact sensor based on :class:`isaacsim.core.prims.RigidContactView`.""" + +from .contact_sensor import ContactSensor +from .contact_sensor_cfg import ContactSensorCfg +from .contact_sensor_data import ContactSensorData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor.py new file mode 100644 index 00000000000..ee82cc36654 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -0,0 +1,427 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Ignore optional memory usage warning globally +# pyright: reportOptionalSubscript=false + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import carb +import omni.physics.tensors.impl.api as physx +from isaacsim.core.simulation_manager import SimulationManager +from pxr import PhysxSchema + +import isaaclab.sim as sim_utils +import isaaclab.utils.string as string_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.utils.math import convert_quat + +from ..sensor_base import SensorBase +from .contact_sensor_data import ContactSensorData + +if TYPE_CHECKING: + from .contact_sensor_cfg import ContactSensorCfg + + +class ContactSensor(SensorBase): + """A contact reporting sensor. + + The contact sensor reports the normal contact forces on a rigid body in the world frame. + It relies on the `PhysX ContactReporter`_ API to be activated on the rigid bodies. + + To enable the contact reporter on a rigid body, please make sure to enable the + :attr:`isaaclab.sim.spawner.RigidObjectSpawnerCfg.activate_contact_sensors` on your + asset spawner configuration. This will enable the contact reporter on all the rigid bodies + in the asset. + + The sensor can be configured to report the contact forces on a set of bodies with a given + filter pattern using the :attr:`ContactSensorCfg.filter_prim_paths_expr`. This is useful + when you want to report the contact forces between the sensor bodies and a specific set of + bodies in the scene. The data can be accessed using the :attr:`ContactSensorData.force_matrix_w`. + Please check the documentation on `RigidContact`_ for more details. + + The reporting of the filtered contact forces is only possible as one-to-many. This means that only one + sensor body in an environment can be filtered against multiple bodies in that environment. If you need to + filter multiple sensor bodies against multiple bodies, you need to create separate sensors for each sensor + body. + + As an example, suppose you want to report the contact forces for all the feet of a robot against an object + exclusively. In that case, setting the :attr:`ContactSensorCfg.prim_path` and + :attr:`ContactSensorCfg.filter_prim_paths_expr` with ``{ENV_REGEX_NS}/Robot/.*_FOOT`` and ``{ENV_REGEX_NS}/Object`` + respectively will not work. Instead, you need to create a separate sensor for each foot and filter + it against the object. + + .. _PhysX ContactReporter: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_contact_report_a_p_i.html + .. _RigidContact: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.core/docs/index.html#isaacsim.core.prims.RigidContact + """ + + cfg: ContactSensorCfg + """The configuration parameters.""" + + def __init__(self, cfg: ContactSensorCfg): + """Initializes the contact sensor object. + + Args: + cfg: The configuration parameters. + """ + # initialize base class + super().__init__(cfg) + + # Enable contact processing + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/physics/disableContactProcessing", False) + + # Create empty variables for storing output data + self._data: ContactSensorData = ContactSensorData() + # initialize self._body_physx_view for running in extension mode + self._body_physx_view = None + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Contact sensor @ '{self.cfg.prim_path}': \n" + f"\tview type : {self.body_physx_view.__class__}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of bodies : {self.num_bodies}\n" + f"\tbody names : {self.body_names}\n" + ) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self.body_physx_view.count + + @property + def data(self) -> ContactSensorData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def num_bodies(self) -> int: + """Number of bodies with contact sensors attached.""" + return self._num_bodies + + @property + def body_names(self) -> list[str]: + """Ordered names of bodies with contact sensors attached.""" + prim_paths = self.body_physx_view.prim_paths[: self.num_bodies] + return [path.split("/")[-1] for path in prim_paths] + + @property + def body_physx_view(self) -> physx.RigidBodyView: + """View for the rigid bodies captured (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._body_physx_view + + @property + def contact_physx_view(self) -> physx.RigidContactView: + """Contact reporter view for the bodies (PhysX). + + Note: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._contact_physx_view + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # reset the timers and counters + super().reset(env_ids) + # resolve None + if env_ids is None: + env_ids = slice(None) + # reset accumulative data buffers + self._data.net_forces_w[env_ids] = 0.0 + self._data.net_forces_w_history[env_ids] = 0.0 + if self.cfg.history_length > 0: + self._data.net_forces_w_history[env_ids] = 0.0 + # reset force matrix + if len(self.cfg.filter_prim_paths_expr) != 0: + self._data.force_matrix_w[env_ids] = 0.0 + # reset the current air time + if self.cfg.track_air_time: + self._data.current_air_time[env_ids] = 0.0 + self._data.last_air_time[env_ids] = 0.0 + self._data.current_contact_time[env_ids] = 0.0 + self._data.last_contact_time[env_ids] = 0.0 + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the articulation based on the name keys. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self.body_names, preserve_order) + + def compute_first_contact(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: + """Checks if bodies that have established contact within the last :attr:`dt` seconds. + + This function checks if the bodies have established contact within the last :attr:`dt` seconds + by comparing the current contact time with the given time period. If the contact time is less + than the given time period, then the bodies are considered to be in contact. + + Note: + The function assumes that :attr:`dt` is a factor of the sensor update time-step. In other + words :math:`dt / dt_sensor = n`, where :math:`n` is a natural number. This is always true + if the sensor is updated by the physics or the environment stepping time-step and the sensor + is read by the environment stepping time-step. + + Args: + dt: The time period since the contact was established. + abs_tol: The absolute tolerance for the comparison. + + Returns: + A boolean tensor indicating the bodies that have established contact within the last + :attr:`dt` seconds. Shape is (N, B), where N is the number of sensors and B is the + number of bodies in each sensor. + + Raises: + RuntimeError: If the sensor is not configured to track contact time. + """ + # check if the sensor is configured to track contact time + if not self.cfg.track_air_time: + raise RuntimeError( + "The contact sensor is not configured to track contact time." + "Please enable the 'track_air_time' in the sensor configuration." + ) + # check if the bodies are in contact + currently_in_contact = self.data.current_contact_time > 0.0 + less_than_dt_in_contact = self.data.current_contact_time < (dt + abs_tol) + return currently_in_contact * less_than_dt_in_contact + + def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: + """Checks if bodies that have broken contact within the last :attr:`dt` seconds. + + This function checks if the bodies have broken contact within the last :attr:`dt` seconds + by comparing the current air time with the given time period. If the air time is less + than the given time period, then the bodies are considered to not be in contact. + + Note: + It assumes that :attr:`dt` is a factor of the sensor update time-step. In other words, + :math:`dt / dt_sensor = n`, where :math:`n` is a natural number. This is always true if + the sensor is updated by the physics or the environment stepping time-step and the sensor + is read by the environment stepping time-step. + + Args: + dt: The time period since the contract is broken. + abs_tol: The absolute tolerance for the comparison. + + Returns: + A boolean tensor indicating the bodies that have broken contact within the last :attr:`dt` seconds. + Shape is (N, B), where N is the number of sensors and B is the number of bodies in each sensor. + + Raises: + RuntimeError: If the sensor is not configured to track contact time. + """ + # check if the sensor is configured to track contact time + if not self.cfg.track_air_time: + raise RuntimeError( + "The contact sensor is not configured to track contact time." + "Please enable the 'track_air_time' in the sensor configuration." + ) + # check if the sensor is configured to track contact time + currently_detached = self.data.current_air_time > 0.0 + less_than_dt_detached = self.data.current_air_time < (dt + abs_tol) + return currently_detached * less_than_dt_detached + + """ + Implementation. + """ + + def _initialize_impl(self): + super()._initialize_impl() + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # check that only rigid bodies are selected + leaf_pattern = self.cfg.prim_path.rsplit("/", 1)[-1] + template_prim_path = self._parent_prims[0].GetPath().pathString + body_names = list() + for prim in sim_utils.find_matching_prims(template_prim_path + "/" + leaf_pattern): + # check if prim has contact reporter API + if prim.HasAPI(PhysxSchema.PhysxContactReportAPI): + prim_path = prim.GetPath().pathString + body_names.append(prim_path.rsplit("/", 1)[-1]) + # check that there is at least one body with contact reporter API + if not body_names: + raise RuntimeError( + f"Sensor at path '{self.cfg.prim_path}' could not find any bodies with contact reporter API." + "\nHINT: Make sure to enable 'activate_contact_sensors' in the corresponding asset spawn configuration." + ) + + # construct regex expression for the body names + body_names_regex = r"(" + "|".join(body_names) + r")" + body_names_regex = f"{self.cfg.prim_path.rsplit('/', 1)[0]}/{body_names_regex}" + # convert regex expressions to glob expressions for PhysX + body_names_glob = body_names_regex.replace(".*", "*") + filter_prim_paths_glob = [expr.replace(".*", "*") for expr in self.cfg.filter_prim_paths_expr] + + # create a rigid prim view for the sensor + self._body_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_glob) + self._contact_physx_view = self._physics_sim_view.create_rigid_contact_view( + body_names_glob, filter_patterns=filter_prim_paths_glob + ) + # resolve the true count of bodies + self._num_bodies = self.body_physx_view.count // self._num_envs + # check that contact reporter succeeded + if self._num_bodies != len(body_names): + raise RuntimeError( + "Failed to initialize contact reporter for specified bodies." + f"\n\tInput prim path : {self.cfg.prim_path}" + f"\n\tResolved prim paths: {body_names_regex}" + ) + + # prepare data buffers + self._data.net_forces_w = torch.zeros(self._num_envs, self._num_bodies, 3, device=self._device) + # optional buffers + # -- history of net forces + if self.cfg.history_length > 0: + self._data.net_forces_w_history = torch.zeros( + self._num_envs, self.cfg.history_length, self._num_bodies, 3, device=self._device + ) + else: + self._data.net_forces_w_history = self._data.net_forces_w.unsqueeze(1) + # -- pose of sensor origins + if self.cfg.track_pose: + self._data.pos_w = torch.zeros(self._num_envs, self._num_bodies, 3, device=self._device) + self._data.quat_w = torch.zeros(self._num_envs, self._num_bodies, 4, device=self._device) + # -- air/contact time between contacts + if self.cfg.track_air_time: + self._data.last_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) + self._data.current_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) + self._data.last_contact_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) + self._data.current_contact_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) + # force matrix: (num_envs, num_bodies, num_filter_shapes, 3) + if len(self.cfg.filter_prim_paths_expr) != 0: + num_filters = self.contact_physx_view.filter_count + self._data.force_matrix_w = torch.zeros( + self._num_envs, self._num_bodies, num_filters, 3, device=self._device + ) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + # default to all sensors + if len(env_ids) == self._num_envs: + env_ids = slice(None) + + # obtain the contact forces + # TODO: We are handling the indexing ourself because of the shape; (N, B) vs expected (N * B). + # This isn't the most efficient way to do this, but it's the easiest to implement. + net_forces_w = self.contact_physx_view.get_net_contact_forces(dt=self._sim_physics_dt) + self._data.net_forces_w[env_ids, :, :] = net_forces_w.view(-1, self._num_bodies, 3)[env_ids] + # update contact force history + if self.cfg.history_length > 0: + self._data.net_forces_w_history[env_ids, 1:] = self._data.net_forces_w_history[env_ids, :-1].clone() + self._data.net_forces_w_history[env_ids, 0] = self._data.net_forces_w[env_ids] + + # obtain the contact force matrix + if len(self.cfg.filter_prim_paths_expr) != 0: + # shape of the filtering matrix: (num_envs, num_bodies, num_filter_shapes, 3) + num_filters = self.contact_physx_view.filter_count + # acquire and shape the force matrix + force_matrix_w = self.contact_physx_view.get_contact_force_matrix(dt=self._sim_physics_dt) + force_matrix_w = force_matrix_w.view(-1, self._num_bodies, num_filters, 3) + self._data.force_matrix_w[env_ids] = force_matrix_w[env_ids] + + # obtain the pose of the sensor origin + if self.cfg.track_pose: + pose = self.body_physx_view.get_transforms().view(-1, self._num_bodies, 7)[env_ids] + pose[..., 3:] = convert_quat(pose[..., 3:], to="wxyz") + self._data.pos_w[env_ids], self._data.quat_w[env_ids] = pose.split([3, 4], dim=-1) + + # obtain the air time + if self.cfg.track_air_time: + # -- time elapsed since last update + # since this function is called every frame, we can use the difference to get the elapsed time + elapsed_time = self._timestamp[env_ids] - self._timestamp_last_update[env_ids] + # -- check contact state of bodies + is_contact = torch.norm(self._data.net_forces_w[env_ids, :, :], dim=-1) > self.cfg.force_threshold + is_first_contact = (self._data.current_air_time[env_ids] > 0) * is_contact + is_first_detached = (self._data.current_contact_time[env_ids] > 0) * ~is_contact + # -- update the last contact time if body has just become in contact + self._data.last_air_time[env_ids] = torch.where( + is_first_contact, + self._data.current_air_time[env_ids] + elapsed_time.unsqueeze(-1), + self._data.last_air_time[env_ids], + ) + # -- increment time for bodies that are not in contact + self._data.current_air_time[env_ids] = torch.where( + ~is_contact, self._data.current_air_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 + ) + # -- update the last contact time if body has just detached + self._data.last_contact_time[env_ids] = torch.where( + is_first_detached, + self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), + self._data.last_contact_time[env_ids], + ) + # -- increment time for bodies that are in contact + self._data.current_contact_time[env_ids] = torch.where( + is_contact, self._data.current_contact_time[env_ids] + elapsed_time.unsqueeze(-1), 0.0 + ) + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first tome + if not hasattr(self, "contact_visualizer"): + self.contact_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self.contact_visualizer.set_visibility(True) + else: + if hasattr(self, "contact_visualizer"): + self.contact_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # safely return if view becomes invalid + # note: this invalidity happens because of isaac sim view callbacks + if self.body_physx_view is None: + return + # marker indices + # 0: contact, 1: no contact + net_contact_force_w = torch.norm(self._data.net_forces_w, dim=-1) + marker_indices = torch.where(net_contact_force_w > self.cfg.force_threshold, 0, 1) + # check if prim is visualized + if self.cfg.track_pose: + frame_origins: torch.Tensor = self._data.pos_w + else: + pose = self.body_physx_view.get_transforms() + frame_origins = pose.view(-1, self._num_bodies, 7)[:, :, :3] + # visualize + self.contact_visualizer.visualize(frame_origins.view(-1, 3), marker_indices=marker_indices.view(-1)) + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._body_physx_view = None + self._contact_physx_view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py new file mode 100644 index 00000000000..d18a260a618 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import CONTACT_SENSOR_MARKER_CFG +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg +from .contact_sensor import ContactSensor + + +@configclass +class ContactSensorCfg(SensorBaseCfg): + """Configuration for the contact sensor.""" + + class_type: type = ContactSensor + + track_pose: bool = False + """Whether to track the pose of the sensor's origin. Defaults to False.""" + + track_air_time: bool = False + """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False.""" + + force_threshold: float = 1.0 + """The threshold on the norm of the contact force that determines whether two bodies are in collision or not. + + This value is only used for tracking the mode duration (the time in contact or in air), + if :attr:`track_air_time` is True. + """ + + filter_prim_paths_expr: list[str] = list() + """The list of primitive paths (or expressions) to filter contacts with. Defaults to an empty list, in which case + no filtering is applied. + + The contact sensor allows reporting contacts between the primitive specified with :attr:`prim_path` and + other primitives in the scene. For instance, in a scene containing a robot, a ground plane and an object, + you can obtain individual contact reports of the base of the robot with the ground plane and the object. + + .. note:: + The expression in the list can contain the environment namespace regex ``{ENV_REGEX_NS}`` which + will be replaced with the environment namespace. + + Example: ``{ENV_REGEX_NS}/Object`` will be replaced with ``/World/envs/env_.*/Object``. + + .. attention:: + The reporting of filtered contacts only works when the sensor primitive :attr:`prim_path` corresponds to a + single primitive in that environment. If the sensor primitive corresponds to multiple primitives, the + filtering will not work as expected. Please check :class:`~isaaclab.sensors.contact_sensor.ContactSensor` + for more details. + """ + + visualizer_cfg: VisualizationMarkersCfg = CONTACT_SENSOR_MARKER_CFG.replace(prim_path="/Visuals/ContactSensor") + """The configuration object for the visualization markers. Defaults to CONTACT_SENSOR_MARKER_CFG. + + .. note:: + This attribute is only used when debug visualization is enabled. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_data.py new file mode 100644 index 00000000000..1637c7a3acc --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -0,0 +1,107 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# needed to import for allowing type-hinting: torch.Tensor | None +from __future__ import annotations + +import torch +from dataclasses import dataclass + + +@dataclass +class ContactSensorData: + """Data container for the contact reporting sensor.""" + + pos_w: torch.Tensor | None = None + """Position of the sensor origin in world frame. + + Shape is (N, 3), where N is the number of sensors. + + Note: + If the :attr:`ContactSensorCfg.track_pose` is False, then this quantity is None. + """ + + quat_w: torch.Tensor | None = None + """Orientation of the sensor origin in quaternion (w, x, y, z) in world frame. + + Shape is (N, 4), where N is the number of sensors. + + Note: + If the :attr:`ContactSensorCfg.track_pose` is False, then this quantity is None. + """ + + net_forces_w: torch.Tensor | None = None + """The net normal contact forces in world frame. + + Shape is (N, B, 3), where N is the number of sensors and B is the number of bodies in each sensor. + + Note: + This quantity is the sum of the normal contact forces acting on the sensor bodies. It must not be confused + with the total contact forces acting on the sensor bodies (which also includes the tangential forces). + """ + + net_forces_w_history: torch.Tensor | None = None + """The net normal contact forces in world frame. + + Shape is (N, T, B, 3), where N is the number of sensors, T is the configured history length + and B is the number of bodies in each sensor. + + In the history dimension, the first index is the most recent and the last index is the oldest. + + Note: + This quantity is the sum of the normal contact forces acting on the sensor bodies. It must not be confused + with the total contact forces acting on the sensor bodies (which also includes the tangential forces). + """ + + force_matrix_w: torch.Tensor | None = None + """The normal contact forces filtered between the sensor bodies and filtered bodies in world frame. + + Shape is (N, B, M, 3), where N is the number of sensors, B is number of bodies in each sensor + and ``M`` is the number of filtered bodies. + + Note: + If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None. + """ + + last_air_time: torch.Tensor | None = None + """Time spent (in s) in the air before the last contact. + + Shape is (N, B), where N is the number of sensors and B is the number of bodies in each sensor. + + Note: + If the :attr:`ContactSensorCfg.track_air_time` is False, then this quantity is None. + """ + + current_air_time: torch.Tensor | None = None + """Time spent (in s) in the air since the last detach. + + Shape is (N, B), where N is the number of sensors and B is the number of bodies in each sensor. + + Note: + If the :attr:`ContactSensorCfg.track_air_time` is False, then this quantity is None. + """ + + last_contact_time: torch.Tensor | None = None + """Time spent (in s) in contact before the last detach. + + Shape is (N, B), where N is the number of sensors and B is the number of bodies in each sensor. + + Note: + If the :attr:`ContactSensorCfg.track_air_time` is False, then this quantity is None. + """ + + current_contact_time: torch.Tensor | None = None + """Time spent (in s) in contact since the last contact. + + Shape is (N, B), where N is the number of sensors and B is the number of bodies in each sensor. + + Note: + If the :attr:`ContactSensorCfg.track_air_time` is False, then this quantity is None. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/__init__.py new file mode 100644 index 00000000000..3794c329fd2 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for frame transformer sensor.""" + +from .frame_transformer import FrameTransformer +from .frame_transformer_cfg import FrameTransformerCfg, OffsetCfg +from .frame_transformer_data import FrameTransformerData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer.py new file mode 100644 index 00000000000..ba15bf8776f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -0,0 +1,473 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import re +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log +from isaacsim.core.simulation_manager import SimulationManager +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.string as string_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.utils.math import combine_frame_transforms, convert_quat, is_identity_pose, subtract_frame_transforms + +from ..sensor_base import SensorBase +from .frame_transformer_data import FrameTransformerData + +if TYPE_CHECKING: + from .frame_transformer_cfg import FrameTransformerCfg + + +class FrameTransformer(SensorBase): + """A sensor for reporting frame transforms. + + This class provides an interface for reporting the transform of one or more frames (target frames) + with respect to another frame (source frame). The source frame is specified by the user as a prim path + (:attr:`FrameTransformerCfg.prim_path`) and the target frames are specified by the user as a list of + prim paths (:attr:`FrameTransformerCfg.target_frames`). + + The source frame and target frames are assumed to be rigid bodies. The transform of the target frames + with respect to the source frame is computed by first extracting the transform of the source frame + and target frames from the physics engine and then computing the relative transform between the two. + + Additionally, the user can specify an offset for the source frame and each target frame. This is useful + for specifying the transform of the desired frame with respect to the body's center of mass, for instance. + + A common example of using this sensor is to track the position and orientation of the end effector of a + robotic manipulator. In this case, the source frame would be the body corresponding to the base frame of the + manipulator, and the target frame would be the body corresponding to the end effector. Since the end-effector is + typically a fictitious body, the user may need to specify an offset from the end-effector to the body of the + manipulator. + + """ + + cfg: FrameTransformerCfg + """The configuration parameters.""" + + def __init__(self, cfg: FrameTransformerCfg): + """Initializes the frame transformer object. + + Args: + cfg: The configuration parameters. + """ + # initialize base class + super().__init__(cfg) + # Create empty variables for storing output data + self._data: FrameTransformerData = FrameTransformerData() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"FrameTransformer @ '{self.cfg.prim_path}': \n" + f"\ttracked body frames: {[self._source_frame_body_name] + self._target_frame_body_names} \n" + f"\tnumber of envs: {self._num_envs}\n" + f"\tsource body frame: {self._source_frame_body_name}\n" + f"\ttarget frames (count: {self._target_frame_names}): {len(self._target_frame_names)}\n" + ) + + """ + Properties + """ + + @property + def data(self) -> FrameTransformerData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def num_bodies(self) -> int: + """Returns the number of target bodies being tracked. + + Note: + This is an alias used for consistency with other sensors. Otherwise, we recommend using + :attr:`len(data.target_frame_names)` to access the number of target frames. + """ + return len(self._target_frame_body_names) + + @property + def body_names(self) -> list[str]: + """Returns the names of the target bodies being tracked. + + Note: + This is an alias used for consistency with other sensors. Otherwise, we recommend using + :attr:`data.target_frame_names` to access the target frame names. + """ + return self._target_frame_body_names + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # reset the timers and counters + super().reset(env_ids) + # resolve None + if env_ids is None: + env_ids = ... + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the articulation based on the name keys. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return string_utils.resolve_matching_names(name_keys, self._target_frame_names, preserve_order) + + """ + Implementation. + """ + + def _initialize_impl(self): + super()._initialize_impl() + + # resolve source frame offset + source_frame_offset_pos = torch.tensor(self.cfg.source_frame_offset.pos, device=self.device) + source_frame_offset_quat = torch.tensor(self.cfg.source_frame_offset.rot, device=self.device) + # Only need to perform offsetting of source frame if the position offsets is non-zero and rotation offset is + # not the identity quaternion for efficiency in _update_buffer_impl + self._apply_source_frame_offset = True + # Handle source frame offsets + if is_identity_pose(source_frame_offset_pos, source_frame_offset_quat): + omni.log.verbose(f"No offset application needed for source frame as it is identity: {self.cfg.prim_path}") + self._apply_source_frame_offset = False + else: + omni.log.verbose(f"Applying offset to source frame as it is not identity: {self.cfg.prim_path}") + # Store offsets as tensors (duplicating each env's offsets for ease of multiplication later) + self._source_frame_offset_pos = source_frame_offset_pos.unsqueeze(0).repeat(self._num_envs, 1) + self._source_frame_offset_quat = source_frame_offset_quat.unsqueeze(0).repeat(self._num_envs, 1) + + # Keep track of mapping from the rigid body name to the desired frames and prim path, as there may be multiple frames + # based upon the same body name and we don't want to create unnecessary views + body_names_to_frames: dict[str, dict[str, set[str] | str]] = {} + # The offsets associated with each target frame + target_offsets: dict[str, dict[str, torch.Tensor]] = {} + # The frames whose offsets are not identity + non_identity_offset_frames: list[str] = [] + + # Only need to perform offsetting of target frame if any of the position offsets are non-zero or any of the + # rotation offsets are not the identity quaternion for efficiency in _update_buffer_impl + self._apply_target_frame_offset = False + + # Need to keep track of whether the source frame is also a target frame + self._source_is_also_target_frame = False + + # Collect all target frames, their associated body prim paths and their offsets so that we can extract + # the prim, check that it has the appropriate rigid body API in a single loop. + # First element is None because user can't specify source frame name + frames = [None] + [target_frame.name for target_frame in self.cfg.target_frames] + frame_prim_paths = [self.cfg.prim_path] + [target_frame.prim_path for target_frame in self.cfg.target_frames] + # First element is None because source frame offset is handled separately + frame_offsets = [None] + [target_frame.offset for target_frame in self.cfg.target_frames] + frame_types = ["source"] + ["target"] * len(self.cfg.target_frames) + for frame, prim_path, offset, frame_type in zip(frames, frame_prim_paths, frame_offsets, frame_types): + # Find correct prim + matching_prims = sim_utils.find_matching_prims(prim_path) + if len(matching_prims) == 0: + raise ValueError( + f"Failed to create frame transformer for frame '{frame}' with path '{prim_path}'." + " No matching prims were found." + ) + for prim in matching_prims: + # Get the prim path of the matching prim + matching_prim_path = prim.GetPath().pathString + # Check if it is a rigid prim + if not prim.HasAPI(UsdPhysics.RigidBodyAPI): + raise ValueError( + f"While resolving expression '{prim_path}' found a prim '{matching_prim_path}' which is not a" + " rigid body. The class only supports transformations between rigid bodies." + ) + + # Get the name of the body + body_name = matching_prim_path.rsplit("/", 1)[-1] + # Use body name if frame isn't specified by user + frame_name = frame if frame is not None else body_name + + # Keep track of which frames are associated with which bodies + if body_name in body_names_to_frames: + body_names_to_frames[body_name]["frames"].add(frame_name) + + # This is a corner case where the source frame is also a target frame + if body_names_to_frames[body_name]["type"] == "source" and frame_type == "target": + self._source_is_also_target_frame = True + + else: + # Store the first matching prim path and the type of frame + body_names_to_frames[body_name] = { + "frames": {frame_name}, + "prim_path": matching_prim_path, + "type": frame_type, + } + + if offset is not None: + offset_pos = torch.tensor(offset.pos, device=self.device) + offset_quat = torch.tensor(offset.rot, device=self.device) + # Check if we need to apply offsets (optimized code path in _update_buffer_impl) + if not is_identity_pose(offset_pos, offset_quat): + non_identity_offset_frames.append(frame_name) + self._apply_target_frame_offset = True + target_offsets[frame_name] = {"pos": offset_pos, "quat": offset_quat} + + if not self._apply_target_frame_offset: + omni.log.info( + f"No offsets application needed from '{self.cfg.prim_path}' to target frames as all" + f" are identity: {frames[1:]}" + ) + else: + omni.log.info( + f"Offsets application needed from '{self.cfg.prim_path}' to the following target frames:" + f" {non_identity_offset_frames}" + ) + + # The names of bodies that RigidPrim will be tracking to later extract transforms from + tracked_prim_paths = [body_names_to_frames[body_name]["prim_path"] for body_name in body_names_to_frames.keys()] + tracked_body_names = [body_name for body_name in body_names_to_frames.keys()] + + body_names_regex = [tracked_prim_path.replace("env_0", "env_*") for tracked_prim_path in tracked_prim_paths] + + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # Create a prim view for all frames and initialize it + # order of transforms coming out of view will be source frame followed by target frame(s) + self._frame_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_regex) + + # Determine the order in which regex evaluated body names so we can later index into frame transforms + # by frame name correctly + all_prim_paths = self._frame_physx_view.prim_paths + + if "env_" in all_prim_paths[0]: + + def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: + """Separates the environment number and prim_path from the item. + + Args: + item: The item to extract the environment number from. Assumes item is of the form + `/World/envs/env_1/blah` or `/World/envs/env_11/blah`. + Returns: + The environment number and the prim_path. + """ + match = re.search(r"env_(\d+)(.*)", item) + return (int(match.group(1)), match.group(2)) + + # Find the indices that would reorganize output to be per environment. We want `env_1/blah` to come before `env_11/blah` + # and env_1/Robot/base to come before env_1/Robot/foot so we need to use custom key function + self._per_env_indices = [ + index + for index, _ in sorted( + list(enumerate(all_prim_paths)), key=lambda x: extract_env_num_and_prim_path(x[1]) + ) + ] + + # Only need 0th env as the names and their ordering are the same across environments + sorted_prim_paths = [ + all_prim_paths[index] for index in self._per_env_indices if "env_0" in all_prim_paths[index] + ] + + else: + # If no environment is present, then the order of the body names is the same as the order of the prim paths sorted alphabetically + self._per_env_indices = [index for index, _ in sorted(enumerate(all_prim_paths), key=lambda x: x[1])] + sorted_prim_paths = [all_prim_paths[index] for index in self._per_env_indices] + + # -- target frames + self._target_frame_body_names = [prim_path.split("/")[-1] for prim_path in sorted_prim_paths] + + # -- source frame + self._source_frame_body_name = self.cfg.prim_path.split("/")[-1] + source_frame_index = self._target_frame_body_names.index(self._source_frame_body_name) + + # Only remove source frame from tracked bodies if it is not also a target frame + if not self._source_is_also_target_frame: + self._target_frame_body_names.remove(self._source_frame_body_name) + + # Determine indices into all tracked body frames for both source and target frames + all_ids = torch.arange(self._num_envs * len(tracked_body_names)) + self._source_frame_body_ids = torch.arange(self._num_envs) * len(tracked_body_names) + source_frame_index + + # If source frame is also a target frame, then the target frame body ids are the same as the source frame body ids + if self._source_is_also_target_frame: + self._target_frame_body_ids = all_ids + else: + self._target_frame_body_ids = all_ids[~torch.isin(all_ids, self._source_frame_body_ids)] + + # The name of each of the target frame(s) - either user specified or defaulted to the body name + self._target_frame_names: list[str] = [] + # The position and rotation components of target frame offsets + target_frame_offset_pos = [] + target_frame_offset_quat = [] + # Stores the indices of bodies that need to be duplicated. For instance, if body "LF_SHANK" is needed + # for 2 frames, this list enables us to duplicate the body to both frames when doing the calculations + # when updating sensor in _update_buffers_impl + duplicate_frame_indices = [] + + # Go through each body name and determine the number of duplicates we need for that frame + # and extract the offsets. This is all done to handle the case where multiple frames + # reference the same body, but have different names and/or offsets + for i, body_name in enumerate(self._target_frame_body_names): + for frame in body_names_to_frames[body_name]["frames"]: + # Only need to handle target frames here as source frame is handled separately + if frame in target_offsets: + target_frame_offset_pos.append(target_offsets[frame]["pos"]) + target_frame_offset_quat.append(target_offsets[frame]["quat"]) + self._target_frame_names.append(frame) + duplicate_frame_indices.append(i) + + # To handle multiple environments, need to expand so [0, 1, 1, 2] with 2 environments becomes + # [0, 1, 1, 2, 3, 4, 4, 5]. Again, this is a optimization to make _update_buffer_impl more efficient + duplicate_frame_indices = torch.tensor(duplicate_frame_indices, device=self.device) + if self._source_is_also_target_frame: + num_target_body_frames = len(tracked_body_names) + else: + num_target_body_frames = len(tracked_body_names) - 1 + + self._duplicate_frame_indices = torch.cat( + [duplicate_frame_indices + num_target_body_frames * env_num for env_num in range(self._num_envs)] + ) + + # Target frame offsets are only applied if at least one of the offsets are non-identity + if self._apply_target_frame_offset: + # Stack up all the frame offsets for shape (num_envs, num_frames, 3) and (num_envs, num_frames, 4) + self._target_frame_offset_pos = torch.stack(target_frame_offset_pos).repeat(self._num_envs, 1) + self._target_frame_offset_quat = torch.stack(target_frame_offset_quat).repeat(self._num_envs, 1) + + # fill the data buffer + self._data.target_frame_names = self._target_frame_names + self._data.source_pos_w = torch.zeros(self._num_envs, 3, device=self._device) + self._data.source_quat_w = torch.zeros(self._num_envs, 4, device=self._device) + self._data.target_pos_w = torch.zeros(self._num_envs, len(duplicate_frame_indices), 3, device=self._device) + self._data.target_quat_w = torch.zeros(self._num_envs, len(duplicate_frame_indices), 4, device=self._device) + self._data.target_pos_source = torch.zeros_like(self._data.target_pos_w) + self._data.target_quat_source = torch.zeros_like(self._data.target_quat_w) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + # default to all sensors + if len(env_ids) == self._num_envs: + env_ids = ... + + # Extract transforms from view - shape is: + # (the total number of source and target body frames being tracked * self._num_envs, 7) + transforms = self._frame_physx_view.get_transforms() + + # Reorder the transforms to be per environment as is expected of SensorData + transforms = transforms[self._per_env_indices] + + # Convert quaternions as PhysX uses xyzw form + transforms[:, 3:] = convert_quat(transforms[:, 3:], to="wxyz") + + # Process source frame transform + source_frames = transforms[self._source_frame_body_ids] + # Only apply offset if the offsets will result in a coordinate frame transform + if self._apply_source_frame_offset: + source_pos_w, source_quat_w = combine_frame_transforms( + source_frames[:, :3], + source_frames[:, 3:], + self._source_frame_offset_pos, + self._source_frame_offset_quat, + ) + else: + source_pos_w = source_frames[:, :3] + source_quat_w = source_frames[:, 3:] + + # Process target frame transforms + target_frames = transforms[self._target_frame_body_ids] + duplicated_target_frame_pos_w = target_frames[self._duplicate_frame_indices, :3] + duplicated_target_frame_quat_w = target_frames[self._duplicate_frame_indices, 3:] + + # Only apply offset if the offsets will result in a coordinate frame transform + if self._apply_target_frame_offset: + target_pos_w, target_quat_w = combine_frame_transforms( + duplicated_target_frame_pos_w, + duplicated_target_frame_quat_w, + self._target_frame_offset_pos, + self._target_frame_offset_quat, + ) + else: + target_pos_w = duplicated_target_frame_pos_w + target_quat_w = duplicated_target_frame_quat_w + + # Compute the transform of the target frame with respect to the source frame + total_num_frames = len(self._target_frame_names) + target_pos_source, target_quat_source = subtract_frame_transforms( + source_pos_w.unsqueeze(1).expand(-1, total_num_frames, -1).reshape(-1, 3), + source_quat_w.unsqueeze(1).expand(-1, total_num_frames, -1).reshape(-1, 4), + target_pos_w, + target_quat_w, + ) + + # Update buffers + # note: The frame names / ordering don't change so no need to update them after initialization + self._data.source_pos_w[:] = source_pos_w.view(-1, 3) + self._data.source_quat_w[:] = source_quat_w.view(-1, 4) + self._data.target_pos_w[:] = target_pos_w.view(-1, total_num_frames, 3) + self._data.target_quat_w[:] = target_quat_w.view(-1, total_num_frames, 4) + self._data.target_pos_source[:] = target_pos_source.view(-1, total_num_frames, 3) + self._data.target_quat_source[:] = target_quat_source.view(-1, total_num_frames, 4) + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + if not hasattr(self, "frame_visualizer"): + self.frame_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + + try: + # isaacsim.util is not available in headless mode + import isaacsim.util.debug_draw._debug_draw as isaac_debug_draw + + self.debug_draw = isaac_debug_draw.acquire_debug_draw_interface() + except ImportError: + omni.log.info("isaacsim.util.debug_draw module not found. Debug visualization will be limited.") + + # set their visibility to true + self.frame_visualizer.set_visibility(True) + else: + if hasattr(self, "frame_visualizer"): + self.frame_visualizer.set_visibility(False) + # clear the lines + if hasattr(self, "debug_draw"): + self.debug_draw.clear_lines() + + def _debug_vis_callback(self, event): + # Update the visualized markers + all_pos = torch.cat([self._data.source_pos_w, self._data.target_pos_w.view(-1, 3)], dim=0) + all_quat = torch.cat([self._data.source_quat_w, self._data.target_quat_w.view(-1, 4)], dim=0) + self.frame_visualizer.visualize(all_pos, all_quat) + + if hasattr(self, "debug_draw"): + # Draw lines connecting the source frame to the target frames + self.debug_draw.clear_lines() + # make the lines color yellow + source_pos = self._data.source_pos_w.cpu().tolist() + colors = [[1, 1, 0, 1]] * self._num_envs + for frame_index in range(len(self._target_frame_names)): + target_pos = self._data.target_pos_w[:, frame_index].cpu().tolist() + self.debug_draw.draw_lines(source_pos, target_pos, colors, [1.5] * self._num_envs) + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._frame_physx_view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py new file mode 100644 index 00000000000..f83cd5a607b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_cfg.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.markers.config import FRAME_MARKER_CFG, VisualizationMarkersCfg +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg +from .frame_transformer import FrameTransformer + + +@configclass +class OffsetCfg: + """The offset pose of one frame relative to another frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + +@configclass +class FrameTransformerCfg(SensorBaseCfg): + """Configuration for the frame transformer sensor.""" + + @configclass + class FrameCfg: + """Information specific to a coordinate frame.""" + + prim_path: str = MISSING + """The prim path corresponding to a rigid body. + + This can be a regex pattern to match multiple prims. For example, "/Robot/.*" will match all prims under "/Robot". + + This means that if the source :attr:`FrameTransformerCfg.prim_path` is "/Robot/base", and the target :attr:`FrameTransformerCfg.FrameCfg.prim_path` is "/Robot/.*", + then the frame transformer will track the poses of all the prims under "/Robot", + including "/Robot/base" (even though this will result in an identity pose w.r.t. the source frame). + """ + + name: str | None = None + """User-defined name for the new coordinate frame. Defaults to None. + + If None, then the name is extracted from the leaf of the prim path. + """ + + offset: OffsetCfg = OffsetCfg() + """The pose offset from the parent prim frame.""" + + class_type: type = FrameTransformer + + prim_path: str = MISSING + """The prim path of the body to transform from (source frame).""" + + source_frame_offset: OffsetCfg = OffsetCfg() + """The pose offset from the source prim frame.""" + + target_frames: list[FrameCfg] = MISSING + """A list of the target frames. + + This allows a single FrameTransformer to handle multiple target prims. For example, in a quadruped, + we can use a single FrameTransformer to track each foot's position and orientation in the body + frame using four frame offsets. + """ + + visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace(prim_path="/Visuals/FrameTransformer") + """The configuration object for the visualization markers. Defaults to FRAME_MARKER_CFG. + + Note: + This attribute is only used when debug visualization is enabled. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_data.py new file mode 100644 index 00000000000..4c466cb601e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/frame_transformer/frame_transformer_data.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class FrameTransformerData: + """Data container for the frame transformer sensor.""" + + target_frame_names: list[str] = None + """Target frame names (this denotes the order in which that frame data is ordered). + + The frame names are resolved from the :attr:`FrameTransformerCfg.FrameCfg.name` field. + This does not necessarily follow the order in which the frames are defined in the config due to + the regex matching. + """ + + target_pos_source: torch.Tensor = None + """Position of the target frame(s) relative to source frame. + + Shape is (N, M, 3), where N is the number of environments, and M is the number of target frames. + """ + + target_quat_source: torch.Tensor = None + """Orientation of the target frame(s) relative to source frame quaternion (w, x, y, z). + + Shape is (N, M, 4), where N is the number of environments, and M is the number of target frames. + """ + + target_pos_w: torch.Tensor = None + """Position of the target frame(s) after offset (in world frame). + + Shape is (N, M, 3), where N is the number of environments, and M is the number of target frames. + """ + + target_quat_w: torch.Tensor = None + """Orientation of the target frame(s) after offset (in world frame) quaternion (w, x, y, z). + + Shape is (N, M, 4), where N is the number of environments, and M is the number of target frames. + """ + + source_pos_w: torch.Tensor = None + """Position of the source frame after offset (in world frame). + + Shape is (N, 3), where N is the number of environments. + """ + + source_quat_w: torch.Tensor = None + """Orientation of the source frame after offset (in world frame) quaternion (w, x, y, z). + + Shape is (N, 4), where N is the number of environments. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/__init__.py new file mode 100644 index 00000000000..86dd23249c7 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Imu Sensor +""" + +from .imu import Imu +from .imu_cfg import ImuCfg +from .imu_data import ImuData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu.py new file mode 100644 index 00000000000..f96ba0cdd45 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu.py @@ -0,0 +1,247 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaacsim.core.utils.stage as stage_utils +from isaacsim.core.simulation_manager import SimulationManager +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.markers import VisualizationMarkers + +from ..sensor_base import SensorBase +from .imu_data import ImuData + +if TYPE_CHECKING: + from .imu_cfg import ImuCfg + + +class Imu(SensorBase): + """The Inertia Measurement Unit (IMU) sensor. + + The sensor can be attached to any :class:`RigidObject` or :class:`Articulation` in the scene. The sensor provides complete state information. + The sensor is primarily used to provide the linear acceleration and angular velocity of the object in the body frame. The sensor also provides + the position and orientation of the object in the world frame and the angular acceleration and linear velocity in the body frame. The extra + data outputs are useful for simulating with or comparing against "perfect" state estimation. + + .. note:: + + We are computing the accelerations using numerical differentiation from the velocities. Consequently, the + IMU sensor accuracy depends on the chosen phsyx timestep. For a sufficient accuracy, we recommend to keep the + timestep at least as 200Hz. + + .. note:: + + It is suggested to use the OffsetCfg to define an IMU frame relative to a rigid body prim defined at the root of + a :class:`RigidObject` or a prim that is defined by a non-fixed joint in an :class:`Articulation` (except for the + root of a fixed based articulation). The use frames with fixed joints and small mass/inertia to emulate a transform + relative to a body frame can result in lower performance and accuracy. + + """ + + cfg: ImuCfg + """The configuration parameters.""" + + def __init__(self, cfg: ImuCfg): + """Initializes the Imu sensor. + + Args: + cfg: The configuration parameters. + """ + # initialize base class + super().__init__(cfg) + # Create empty variables for storing output data + self._data = ImuData() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Imu sensor @ '{self.cfg.prim_path}': \n" + f"\tview type : {self._view.__class__}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of sensors : {self._view.count}\n" + ) + + """ + Properties + """ + + @property + def data(self) -> ImuData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def num_instances(self) -> int: + return self._view.count + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # reset the timestamps + super().reset(env_ids) + # resolve None + if env_ids is None: + env_ids = slice(None) + # reset accumulative data buffers + self._data.quat_w[env_ids] = 0.0 + self._data.lin_vel_b[env_ids] = 0.0 + self._data.ang_vel_b[env_ids] = 0.0 + self._data.lin_acc_b[env_ids] = 0.0 + self._data.ang_acc_b[env_ids] = 0.0 + + def update(self, dt: float, force_recompute: bool = False): + # save timestamp + self._dt = dt + # execute updating + super().update(dt, force_recompute) + + """ + Implementation. + """ + + def _initialize_impl(self): + """Initializes the sensor handles and internal buffers. + + This function creates handles and registers the provided data types with the replicator registry to + be able to access the data from the sensor. It also initializes the internal buffers to store the data. + + Raises: + RuntimeError: If the imu prim is not a RigidBodyPrim + """ + # Initialize parent class + super()._initialize_impl() + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # check if the prim at path is a rigid prim + prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if prim is None: + raise RuntimeError(f"Failed to find a prim at path expression: {self.cfg.prim_path}") + # check if it is a RigidBody Prim + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + self._view = self._physics_sim_view.create_rigid_body_view(self.cfg.prim_path.replace(".*", "*")) + else: + raise RuntimeError(f"Failed to find a RigidBodyAPI for the prim paths: {self.cfg.prim_path}") + + # Create internal buffers + self._initialize_buffers_impl() + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + # check if self._dt is set (this is set in the update function) + if not hasattr(self, "_dt"): + raise RuntimeError( + "The update function must be called before the data buffers are accessed the first time." + ) + # default to all sensors + if len(env_ids) == self._num_envs: + env_ids = slice(None) + # obtain the poses of the sensors + pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = math_utils.convert_quat(quat_w, to="wxyz") + + # store the poses + self._data.pos_w[env_ids] = pos_w + math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids]) + self._data.quat_w[env_ids] = math_utils.quat_mul(quat_w, self._offset_quat_b[env_ids]) + + # get the offset from COM to link origin + com_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0] + + # obtain the velocities of the link COM + lin_vel_w, ang_vel_w = self._view.get_velocities()[env_ids].split([3, 3], dim=-1) + # if an offset is present or the COM does not agree with the link origin, the linear velocity has to be + # transformed taking the angular velocity into account + lin_vel_w += torch.linalg.cross( + ang_vel_w, math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1 + ) + + # numerical derivative + lin_acc_w = (lin_vel_w - self._prev_lin_vel_w[env_ids]) / self._dt + self._gravity_bias_w[env_ids] + ang_acc_w = (ang_vel_w - self._prev_ang_vel_w[env_ids]) / self._dt + # store the velocities + self._data.lin_vel_b[env_ids] = math_utils.quat_apply_inverse(self._data.quat_w[env_ids], lin_vel_w) + self._data.ang_vel_b[env_ids] = math_utils.quat_apply_inverse(self._data.quat_w[env_ids], ang_vel_w) + # store the accelerations + self._data.lin_acc_b[env_ids] = math_utils.quat_apply_inverse(self._data.quat_w[env_ids], lin_acc_w) + self._data.ang_acc_b[env_ids] = math_utils.quat_apply_inverse(self._data.quat_w[env_ids], ang_acc_w) + + self._prev_lin_vel_w[env_ids] = lin_vel_w + self._prev_ang_vel_w[env_ids] = ang_vel_w + + def _initialize_buffers_impl(self): + """Create buffers for storing data.""" + # data buffers + self._data.pos_w = torch.zeros(self._view.count, 3, device=self._device) + self._data.quat_w = torch.zeros(self._view.count, 4, device=self._device) + self._data.quat_w[:, 0] = 1.0 + self._data.lin_vel_b = torch.zeros_like(self._data.pos_w) + self._data.ang_vel_b = torch.zeros_like(self._data.pos_w) + self._data.lin_acc_b = torch.zeros_like(self._data.pos_w) + self._data.ang_acc_b = torch.zeros_like(self._data.pos_w) + self._prev_lin_vel_w = torch.zeros_like(self._data.pos_w) + self._prev_ang_vel_w = torch.zeros_like(self._data.pos_w) + + # store sensor offset transformation + self._offset_pos_b = torch.tensor(list(self.cfg.offset.pos), device=self._device).repeat(self._view.count, 1) + self._offset_quat_b = torch.tensor(list(self.cfg.offset.rot), device=self._device).repeat(self._view.count, 1) + # set gravity bias + self._gravity_bias_w = torch.tensor(list(self.cfg.gravity_bias), device=self._device).repeat( + self._view.count, 1 + ) + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first tome + if not hasattr(self, "acceleration_visualizer"): + self.acceleration_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self.acceleration_visualizer.set_visibility(True) + else: + if hasattr(self, "acceleration_visualizer"): + self.acceleration_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # safely return if view becomes invalid + # note: this invalidity happens because of isaac sim view callbacks + if self._view is None: + return + # get marker location + # -- base state + base_pos_w = self._data.pos_w.clone() + base_pos_w[:, 2] += 0.5 + # -- resolve the scales + default_scale = self.acceleration_visualizer.cfg.markers["arrow"].scale + arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.shape[0], 1) + # get up axis of current stage + up_axis = stage_utils.get_stage_up_axis() + # arrow-direction + quat_opengl = math_utils.quat_from_matrix( + math_utils.create_rotation_matrix_from_view( + self._data.pos_w, + self._data.pos_w + math_utils.quat_apply(self._data.quat_w, self._data.lin_acc_b), + up_axis=up_axis, + device=self._device, + ) + ) + quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") + # display markers + self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_cfg.py new file mode 100644 index 00000000000..a1b43397ef0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_cfg.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import RED_ARROW_X_MARKER_CFG +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg +from .imu import Imu + + +@configclass +class ImuCfg(SensorBaseCfg): + """Configuration for an Inertial Measurement Unit (IMU) sensor.""" + + class_type: type = Imu + + @configclass + class OffsetCfg: + """The offset pose of the sensor's frame from the sensor's parent frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + offset: OffsetCfg = OffsetCfg() + """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" + + visualizer_cfg: VisualizationMarkersCfg = RED_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Command/velocity_goal") + """The configuration object for the visualization markers. Defaults to RED_ARROW_X_MARKER_CFG. + + This attribute is only used when debug visualization is enabled. + """ + gravity_bias: tuple[float, float, float] = (0.0, 0.0, 9.81) + """The linear acceleration bias applied to the linear acceleration in the world frame (x,y,z). + + Imu sensors typically output a positive gravity acceleration in opposition to the direction of gravity. This + config parameter allows users to subtract that bias if set to (0.,0.,0.). By default this is set to (0.0,0.0,9.81) + which results in a positive acceleration reading in the world Z. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_data.py new file mode 100644 index 00000000000..b47039e9741 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/imu/imu_data.py @@ -0,0 +1,55 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from dataclasses import dataclass + + +@dataclass +class ImuData: + """Data container for the Imu sensor.""" + + pos_w: torch.Tensor = None + """Position of the sensor origin in world frame. + + Shape is (N, 3), where ``N`` is the number of environments. + """ + + quat_w: torch.Tensor = None + """Orientation of the sensor origin in quaternion ``(w, x, y, z)`` in world frame. + + Shape is (N, 4), where ``N`` is the number of environments. + """ + + lin_vel_b: torch.Tensor = None + """IMU frame angular velocity relative to the world expressed in IMU frame. + + Shape is (N, 3), where ``N`` is the number of environments. + """ + + ang_vel_b: torch.Tensor = None + """IMU frame angular velocity relative to the world expressed in IMU frame. + + Shape is (N, 3), where ``N`` is the number of environments. + """ + + lin_acc_b: torch.Tensor = None + """IMU frame linear acceleration relative to the world expressed in IMU frame. + + Shape is (N, 3), where ``N`` is the number of environments. + """ + + ang_acc_b: torch.Tensor = None + """IMU frame angular acceleration relative to the world expressed in IMU frame. + + Shape is (N, 3), where ``N`` is the number of environments. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/__init__.py new file mode 100644 index 00000000000..c49d4131bf0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for Warp-based ray-cast sensor.""" + +from . import patterns +from .ray_caster import RayCaster +from .ray_caster_camera import RayCasterCamera +from .ray_caster_camera_cfg import RayCasterCameraCfg +from .ray_caster_cfg import RayCasterCfg +from .ray_caster_data import RayCasterData diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/__init__.py new file mode 100644 index 00000000000..375a46839a9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for ray-casting patterns used by the ray-caster.""" + +from .patterns import bpearl_pattern, grid_pattern, lidar_pattern, pinhole_camera_pattern +from .patterns_cfg import BpearlPatternCfg, GridPatternCfg, LidarPatternCfg, PatternBaseCfg, PinholeCameraPatternCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns.py new file mode 100644 index 00000000000..efe53a808a0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns.py @@ -0,0 +1,184 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import torch +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from . import patterns_cfg + + +def grid_pattern(cfg: patterns_cfg.GridPatternCfg, device: str) -> tuple[torch.Tensor, torch.Tensor]: + """A regular grid pattern for ray casting. + + The grid pattern is made from rays that are parallel to each other. They span a 2D grid in the sensor's + local coordinates from ``(-length/2, -width/2)`` to ``(length/2, width/2)``, which is defined + by the ``size = (length, width)`` and ``resolution`` parameters in the config. + + Args: + cfg: The configuration instance for the pattern. + device: The device to create the pattern on. + + Returns: + The starting positions and directions of the rays. + + Raises: + ValueError: If the ordering is not "xy" or "yx". + ValueError: If the resolution is less than or equal to 0. + """ + # check valid arguments + if cfg.ordering not in ["xy", "yx"]: + raise ValueError(f"Ordering must be 'xy' or 'yx'. Received: '{cfg.ordering}'.") + if cfg.resolution <= 0: + raise ValueError(f"Resolution must be greater than 0. Received: '{cfg.resolution}'.") + + # resolve mesh grid indexing (note: torch meshgrid is different from numpy meshgrid) + # check: https://github.com/pytorch/pytorch/issues/15301 + indexing = cfg.ordering if cfg.ordering == "xy" else "ij" + # define grid pattern + x = torch.arange(start=-cfg.size[0] / 2, end=cfg.size[0] / 2 + 1.0e-9, step=cfg.resolution, device=device) + y = torch.arange(start=-cfg.size[1] / 2, end=cfg.size[1] / 2 + 1.0e-9, step=cfg.resolution, device=device) + grid_x, grid_y = torch.meshgrid(x, y, indexing=indexing) + + # store into ray starts + num_rays = grid_x.numel() + ray_starts = torch.zeros(num_rays, 3, device=device) + ray_starts[:, 0] = grid_x.flatten() + ray_starts[:, 1] = grid_y.flatten() + + # define ray-cast directions + ray_directions = torch.zeros_like(ray_starts) + ray_directions[..., :] = torch.tensor(list(cfg.direction), device=device) + + return ray_starts, ray_directions + + +def pinhole_camera_pattern( + cfg: patterns_cfg.PinholeCameraPatternCfg, intrinsic_matrices: torch.Tensor, device: str +) -> tuple[torch.Tensor, torch.Tensor]: + """The image pattern for ray casting. + + .. caution:: + This function does not follow the standard pattern interface. It requires the intrinsic matrices + of the cameras to be passed in. This is because we want to be able to randomize the intrinsic + matrices of the cameras, which is not possible with the standard pattern interface. + + Args: + cfg: The configuration instance for the pattern. + intrinsic_matrices: The intrinsic matrices of the cameras. Shape is (N, 3, 3). + device: The device to create the pattern on. + + Returns: + The starting positions and directions of the rays. The shape of the tensors are + (N, H * W, 3) and (N, H * W, 3) respectively. + """ + # get image plane mesh grid + grid = torch.meshgrid( + torch.arange(start=0, end=cfg.width, dtype=torch.int32, device=device), + torch.arange(start=0, end=cfg.height, dtype=torch.int32, device=device), + indexing="xy", + ) + pixels = torch.vstack(list(map(torch.ravel, grid))).T + # convert to homogeneous coordinate system + pixels = torch.hstack([pixels, torch.ones((len(pixels), 1), device=device)]) + # move each pixel coordinate to the center of the pixel + pixels += torch.tensor([[0.5, 0.5, 0]], device=device) + # get pixel coordinates in camera frame + pix_in_cam_frame = torch.matmul(torch.inverse(intrinsic_matrices), pixels.T) + + # robotics camera frame is (x forward, y left, z up) from camera frame with (x right, y down, z forward) + # transform to robotics camera frame + transform_vec = torch.tensor([1, -1, -1], device=device).unsqueeze(0).unsqueeze(2) + pix_in_cam_frame = pix_in_cam_frame[:, [2, 0, 1], :] * transform_vec + # normalize ray directions + ray_directions = (pix_in_cam_frame / torch.norm(pix_in_cam_frame, dim=1, keepdim=True)).permute(0, 2, 1) + # for camera, we always ray-cast from the sensor's origin + ray_starts = torch.zeros_like(ray_directions, device=device) + + return ray_starts, ray_directions + + +def bpearl_pattern(cfg: patterns_cfg.BpearlPatternCfg, device: str) -> tuple[torch.Tensor, torch.Tensor]: + """The RS-Bpearl pattern for ray casting. + + The `Robosense RS-Bpearl`_ is a short-range LiDAR that has a 360 degrees x 90 degrees super wide + field of view. It is designed for near-field blind-spots detection. + + .. _Robosense RS-Bpearl: https://www.roscomponents.com/en/lidar-laser-scanner/267-rs-bpearl.html + + Args: + cfg: The configuration instance for the pattern. + device: The device to create the pattern on. + + Returns: + The starting positions and directions of the rays. + """ + h = torch.arange(-cfg.horizontal_fov / 2, cfg.horizontal_fov / 2, cfg.horizontal_res, device=device) + v = torch.tensor(list(cfg.vertical_ray_angles), device=device) + + pitch, yaw = torch.meshgrid(v, h, indexing="xy") + pitch, yaw = torch.deg2rad(pitch.reshape(-1)), torch.deg2rad(yaw.reshape(-1)) + pitch += torch.pi / 2 + x = torch.sin(pitch) * torch.cos(yaw) + y = torch.sin(pitch) * torch.sin(yaw) + z = torch.cos(pitch) + + ray_directions = -torch.stack([x, y, z], dim=1) + ray_starts = torch.zeros_like(ray_directions) + return ray_starts, ray_directions + + +def lidar_pattern(cfg: patterns_cfg.LidarPatternCfg, device: str) -> tuple[torch.Tensor, torch.Tensor]: + """Lidar sensor pattern for ray casting. + + Args: + cfg: The configuration instance for the pattern. + device: The device to create the pattern on. + + Returns: + The starting positions and directions of the rays. + """ + # Vertical angles + vertical_angles = torch.linspace(cfg.vertical_fov_range[0], cfg.vertical_fov_range[1], cfg.channels) + + # If the horizontal field of view is 360 degrees, exclude the last point to avoid overlap + if abs(abs(cfg.horizontal_fov_range[0] - cfg.horizontal_fov_range[1]) - 360.0) < 1e-6: + up_to = -1 + else: + up_to = None + + # Horizontal angles + num_horizontal_angles = math.ceil((cfg.horizontal_fov_range[1] - cfg.horizontal_fov_range[0]) / cfg.horizontal_res) + horizontal_angles = torch.linspace(cfg.horizontal_fov_range[0], cfg.horizontal_fov_range[1], num_horizontal_angles)[ + :up_to + ] + + # Convert degrees to radians + vertical_angles_rad = torch.deg2rad(vertical_angles) + horizontal_angles_rad = torch.deg2rad(horizontal_angles) + + # Meshgrid to create a 2D array of angles + v_angles, h_angles = torch.meshgrid(vertical_angles_rad, horizontal_angles_rad, indexing="ij") + + # Spherical to Cartesian conversion (assuming Z is up) + x = torch.cos(v_angles) * torch.cos(h_angles) + y = torch.cos(v_angles) * torch.sin(h_angles) + z = torch.sin(v_angles) + + # Ray directions + ray_directions = torch.stack([x, y, z], dim=-1).reshape(-1, 3).to(device) + + # Ray starts: Assuming all rays originate from (0,0,0) + ray_starts = torch.zeros_like(ray_directions).to(device) + + return ray_starts, ray_directions diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py new file mode 100644 index 00000000000..505221aa14a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/patterns/patterns_cfg.py @@ -0,0 +1,223 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ray-cast sensor.""" + +from __future__ import annotations + +import torch +from collections.abc import Callable, Sequence +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import patterns + + +@configclass +class PatternBaseCfg: + """Base configuration for a pattern.""" + + func: Callable[[PatternBaseCfg, str], tuple[torch.Tensor, torch.Tensor]] = MISSING + """Function to generate the pattern. + + The function should take in the configuration and the device name as arguments. It should return + the pattern's starting positions and directions as a tuple of torch.Tensor. + """ + + +@configclass +class GridPatternCfg(PatternBaseCfg): + """Configuration for the grid pattern for ray-casting. + + Defines a 2D grid of rays in the coordinates of the sensor. + + .. attention:: + The points are ordered based on the :attr:`ordering` attribute. + + """ + + func: Callable = patterns.grid_pattern + + resolution: float = MISSING + """Grid resolution (in meters).""" + + size: tuple[float, float] = MISSING + """Grid size (length, width) (in meters).""" + + direction: tuple[float, float, float] = (0.0, 0.0, -1.0) + """Ray direction. Defaults to (0.0, 0.0, -1.0).""" + + ordering: Literal["xy", "yx"] = "xy" + """Specifies the ordering of points in the generated grid. Defaults to ``"xy"``. + + Consider a grid pattern with points at :math:`(x, y)` where :math:`x` and :math:`y` are the grid indices. + The ordering of the points can be specified as "xy" or "yx". This determines the inner and outer loop order + when iterating over the grid points. + + * If "xy" is selected, the points are ordered with inner loop over "x" and outer loop over "y". + * If "yx" is selected, the points are ordered with inner loop over "y" and outer loop over "x". + + For example, the grid pattern points with :math:`X = (0, 1, 2)` and :math:`Y = (3, 4)`: + + * "xy" ordering: :math:`[(0, 3), (1, 3), (2, 3), (1, 4), (2, 4), (2, 4)]` + * "yx" ordering: :math:`[(0, 3), (0, 4), (1, 3), (1, 4), (2, 3), (2, 4)]` + """ + + +@configclass +class PinholeCameraPatternCfg(PatternBaseCfg): + """Configuration for a pinhole camera depth image pattern for ray-casting. + + .. caution:: + Focal length as well as the aperture sizes and offsets are set as a tenth of the world unit. In our case, the + world unit is meters, so all of these values are in cm. For more information, please check: + https://docs.omniverse.nvidia.com/materials-and-rendering/latest/cameras.html + """ + + func: Callable = patterns.pinhole_camera_pattern + + focal_length: float = 24.0 + """Perspective focal length (in cm). Defaults to 24.0cm. + + Longer lens lengths narrower FOV, shorter lens lengths wider FOV. + """ + + horizontal_aperture: float = 20.955 + """Horizontal aperture (in cm). Defaults to 20.955 cm. + + Emulates sensor/film width on a camera. + + Note: + The default value is the horizontal aperture of a 35 mm spherical projector. + """ + vertical_aperture: float | None = None + r"""Vertical aperture (in cm). Defaults to None. + + Emulates sensor/film height on a camera. If None, then the vertical aperture is calculated based on the + horizontal aperture and the aspect ratio of the image to maintain squared pixels. In this case, the vertical + aperture is calculated as: + + .. math:: + \text{vertical aperture} = \text{horizontal aperture} \times \frac{\text{height}}{\text{width}} + """ + + horizontal_aperture_offset: float = 0.0 + """Offsets Resolution/Film gate horizontally. Defaults to 0.0.""" + + vertical_aperture_offset: float = 0.0 + """Offsets Resolution/Film gate vertically. Defaults to 0.0.""" + + width: int = MISSING + """Width of the image (in pixels).""" + + height: int = MISSING + """Height of the image (in pixels).""" + + @classmethod + def from_intrinsic_matrix( + cls, + intrinsic_matrix: list[float], + width: int, + height: int, + focal_length: float = 24.0, + ) -> PinholeCameraPatternCfg: + r"""Create a :class:`PinholeCameraPatternCfg` class instance from an intrinsic matrix. + + The intrinsic matrix is a 3x3 matrix that defines the mapping between the 3D world coordinates and + the 2D image. The matrix is defined as: + + .. math:: + I_{cam} = \begin{bmatrix} + f_x & 0 & c_x \\ + 0 & f_y & c_y \\ + 0 & 0 & 1 + \end{bmatrix}, + + where :math:`f_x` and :math:`f_y` are the focal length along x and y direction, while :math:`c_x` and :math:`c_y` are the + principle point offsets along x and y direction respectively. + + Args: + intrinsic_matrix: Intrinsic matrix of the camera in row-major format. + The matrix is defined as [f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1]. Shape is (9,). + width: Width of the image (in pixels). + height: Height of the image (in pixels). + focal_length: Focal length of the camera (in cm). Defaults to 24.0 cm. + + Returns: + An instance of the :class:`PinholeCameraPatternCfg` class. + """ + # extract parameters from matrix + f_x = intrinsic_matrix[0] + c_x = intrinsic_matrix[2] + f_y = intrinsic_matrix[4] + c_y = intrinsic_matrix[5] + # resolve parameters for usd camera + horizontal_aperture = width * focal_length / f_x + vertical_aperture = height * focal_length / f_y + horizontal_aperture_offset = (c_x - width / 2) / f_x + vertical_aperture_offset = (c_y - height / 2) / f_y + + return cls( + focal_length=focal_length, + horizontal_aperture=horizontal_aperture, + vertical_aperture=vertical_aperture, + horizontal_aperture_offset=horizontal_aperture_offset, + vertical_aperture_offset=vertical_aperture_offset, + width=width, + height=height, + ) + + +@configclass +class BpearlPatternCfg(PatternBaseCfg): + """Configuration for the Bpearl pattern for ray-casting.""" + + func: Callable = patterns.bpearl_pattern + + horizontal_fov: float = 360.0 + """Horizontal field of view (in degrees). Defaults to 360.0.""" + + horizontal_res: float = 10.0 + """Horizontal resolution (in degrees). Defaults to 10.0.""" + + # fmt: off + vertical_ray_angles: Sequence[float] = [ + 89.5, 86.6875, 83.875, 81.0625, 78.25, 75.4375, 72.625, 69.8125, 67.0, 64.1875, 61.375, + 58.5625, 55.75, 52.9375, 50.125, 47.3125, 44.5, 41.6875, 38.875, 36.0625, 33.25, 30.4375, + 27.625, 24.8125, 22, 19.1875, 16.375, 13.5625, 10.75, 7.9375, 5.125, 2.3125 + ] + # fmt: on + """Vertical ray angles (in degrees). Defaults to a list of 32 angles. + + Note: + We manually set the vertical ray angles to match the Bpearl sensor. The ray-angles + are not evenly spaced. + """ + + +@configclass +class LidarPatternCfg(PatternBaseCfg): + """Configuration for the LiDAR pattern for ray-casting.""" + + func: Callable = patterns.lidar_pattern + + channels: int = MISSING + """Number of Channels (Beams). Determines the vertical resolution of the LiDAR sensor.""" + + vertical_fov_range: tuple[float, float] = MISSING + """Vertical field of view range in degrees.""" + + horizontal_fov_range: tuple[float, float] = MISSING + """Horizontal field of view range in degrees.""" + + horizontal_res: float = MISSING + """Horizontal resolution (in degrees).""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster.py new file mode 100644 index 00000000000..f4dc9918de8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster.py @@ -0,0 +1,295 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import re +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import omni.log +import omni.physics.tensors.impl.api as physx +import warp as wp +from isaacsim.core.prims import XFormPrim +from isaacsim.core.simulation_manager import SimulationManager +from pxr import UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.terrains.trimesh.utils import make_plane +from isaaclab.utils.math import convert_quat, quat_apply, quat_apply_yaw +from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh + +from ..sensor_base import SensorBase +from .ray_caster_data import RayCasterData + +if TYPE_CHECKING: + from .ray_caster_cfg import RayCasterCfg + + +class RayCaster(SensorBase): + """A ray-casting sensor. + + The ray-caster uses a set of rays to detect collisions with meshes in the scene. The rays are + defined in the sensor's local coordinate frame. The sensor can be configured to ray-cast against + a set of meshes with a given ray pattern. + + The meshes are parsed from the list of primitive paths provided in the configuration. These are then + converted to warp meshes and stored in the `warp_meshes` list. The ray-caster then ray-casts against + these warp meshes using the ray pattern provided in the configuration. + + .. note:: + Currently, only static meshes are supported. Extending the warp mesh to support dynamic meshes + is a work in progress. + """ + + cfg: RayCasterCfg + """The configuration parameters.""" + + def __init__(self, cfg: RayCasterCfg): + """Initializes the ray-caster object. + + Args: + cfg: The configuration parameters. + """ + # check if sensor path is valid + # note: currently we do not handle environment indices if there is a regex pattern in the leaf + # For example, if the prim path is "/World/Sensor_[1,2]". + sensor_path = cfg.prim_path.split("/")[-1] + sensor_path_is_regex = re.match(r"^[a-zA-Z0-9/_]+$", sensor_path) is None + if sensor_path_is_regex: + raise RuntimeError( + f"Invalid prim path for the ray-caster sensor: {self.cfg.prim_path}." + "\n\tHint: Please ensure that the prim path does not contain any regex patterns in the leaf." + ) + # Initialize base class + super().__init__(cfg) + # Create empty variables for storing output data + self._data = RayCasterData() + # the warp meshes used for raycasting. + self.meshes: dict[str, wp.Mesh] = {} + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Ray-caster @ '{self.cfg.prim_path}': \n" + f"\tview type : {self._view.__class__}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of meshes : {len(self.meshes)}\n" + f"\tnumber of sensors : {self._view.count}\n" + f"\tnumber of rays/sensor: {self.num_rays}\n" + f"\ttotal number of rays : {self.num_rays * self._view.count}" + ) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self._view.count + + @property + def data(self) -> RayCasterData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + # reset the timers and counters + super().reset(env_ids) + # resolve None + if env_ids is None: + env_ids = slice(None) + # resample the drift + self.drift[env_ids] = self.drift[env_ids].uniform_(*self.cfg.drift_range) + + """ + Implementation. + """ + + def _initialize_impl(self): + super()._initialize_impl() + # obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + # check if the prim at path is an articulated or rigid prim + # we do this since for physics-based view classes we can access their data directly + # otherwise we need to use the xform view class which is slower + found_supported_prim_class = False + prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if prim is None: + raise RuntimeError(f"Failed to find a prim at path expression: {self.cfg.prim_path}") + # create view based on the type of prim + if prim.HasAPI(UsdPhysics.ArticulationRootAPI): + self._view = self._physics_sim_view.create_articulation_view(self.cfg.prim_path.replace(".*", "*")) + found_supported_prim_class = True + elif prim.HasAPI(UsdPhysics.RigidBodyAPI): + self._view = self._physics_sim_view.create_rigid_body_view(self.cfg.prim_path.replace(".*", "*")) + found_supported_prim_class = True + else: + self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + found_supported_prim_class = True + omni.log.warn(f"The prim at path {prim.GetPath().pathString} is not a physics prim! Using XFormPrim.") + # check if prim view class is found + if not found_supported_prim_class: + raise RuntimeError(f"Failed to find a valid prim view class for the prim paths: {self.cfg.prim_path}") + + # load the meshes by parsing the stage + self._initialize_warp_meshes() + # initialize the ray start and directions + self._initialize_rays_impl() + + def _initialize_warp_meshes(self): + # check number of mesh prims provided + if len(self.cfg.mesh_prim_paths) != 1: + raise NotImplementedError( + f"RayCaster currently only supports one mesh prim. Received: {len(self.cfg.mesh_prim_paths)}" + ) + + # read prims to ray-cast + for mesh_prim_path in self.cfg.mesh_prim_paths: + # check if the prim is a plane - handle PhysX plane as a special case + # if a plane exists then we need to create an infinite mesh that is a plane + mesh_prim = sim_utils.get_first_matching_child_prim( + mesh_prim_path, lambda prim: prim.GetTypeName() == "Plane" + ) + # if we did not find a plane then we need to read the mesh + if mesh_prim is None: + # obtain the mesh prim + mesh_prim = sim_utils.get_first_matching_child_prim( + mesh_prim_path, lambda prim: prim.GetTypeName() == "Mesh" + ) + # check if valid + if mesh_prim is None or not mesh_prim.IsValid(): + raise RuntimeError(f"Invalid mesh prim path: {mesh_prim_path}") + # cast into UsdGeomMesh + mesh_prim = UsdGeom.Mesh(mesh_prim) + # read the vertices and faces + points = np.asarray(mesh_prim.GetPointsAttr().Get()) + transform_matrix = np.array(omni.usd.get_world_transform_matrix(mesh_prim)).T + points = np.matmul(points, transform_matrix[:3, :3].T) + points += transform_matrix[:3, 3] + indices = np.asarray(mesh_prim.GetFaceVertexIndicesAttr().Get()) + wp_mesh = convert_to_warp_mesh(points, indices, device=self.device) + # print info + omni.log.info( + f"Read mesh prim: {mesh_prim.GetPath()} with {len(points)} vertices and {len(indices)} faces." + ) + else: + mesh = make_plane(size=(2e6, 2e6), height=0.0, center_zero=True) + wp_mesh = convert_to_warp_mesh(mesh.vertices, mesh.faces, device=self.device) + # print info + omni.log.info(f"Created infinite plane mesh prim: {mesh_prim.GetPath()}.") + # add the warp mesh to the list + self.meshes[mesh_prim_path] = wp_mesh + + # throw an error if no meshes are found + if all([mesh_prim_path not in self.meshes for mesh_prim_path in self.cfg.mesh_prim_paths]): + raise RuntimeError( + f"No meshes found for ray-casting! Please check the mesh prim paths: {self.cfg.mesh_prim_paths}" + ) + + def _initialize_rays_impl(self): + # compute ray stars and directions + self.ray_starts, self.ray_directions = self.cfg.pattern_cfg.func(self.cfg.pattern_cfg, self._device) + self.num_rays = len(self.ray_directions) + # apply offset transformation to the rays + offset_pos = torch.tensor(list(self.cfg.offset.pos), device=self._device) + offset_quat = torch.tensor(list(self.cfg.offset.rot), device=self._device) + self.ray_directions = quat_apply(offset_quat.repeat(len(self.ray_directions), 1), self.ray_directions) + self.ray_starts += offset_pos + # repeat the rays for each sensor + self.ray_starts = self.ray_starts.repeat(self._view.count, 1, 1) + self.ray_directions = self.ray_directions.repeat(self._view.count, 1, 1) + # prepare drift + self.drift = torch.zeros(self._view.count, 3, device=self.device) + # fill the data buffer + self._data.pos_w = torch.zeros(self._view.count, 3, device=self._device) + self._data.quat_w = torch.zeros(self._view.count, 4, device=self._device) + self._data.ray_hits_w = torch.zeros(self._view.count, self.num_rays, 3, device=self._device) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + # obtain the poses of the sensors + if isinstance(self._view, XFormPrim): + pos_w, quat_w = self._view.get_world_poses(env_ids) + elif isinstance(self._view, physx.ArticulationView): + pos_w, quat_w = self._view.get_root_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = convert_quat(quat_w, to="wxyz") + elif isinstance(self._view, physx.RigidBodyView): + pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = convert_quat(quat_w, to="wxyz") + else: + raise RuntimeError(f"Unsupported view type: {type(self._view)}") + # note: we clone here because we are read-only operations + pos_w = pos_w.clone() + quat_w = quat_w.clone() + # apply drift + pos_w += self.drift[env_ids] + # store the poses + self._data.pos_w[env_ids] = pos_w + self._data.quat_w[env_ids] = quat_w + + # ray cast based on the sensor poses + if self.cfg.attach_yaw_only: + # only yaw orientation is considered and directions are not rotated + ray_starts_w = quat_apply_yaw(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids]) + ray_starts_w += pos_w.unsqueeze(1) + ray_directions_w = self.ray_directions[env_ids] + else: + # full orientation is considered + ray_starts_w = quat_apply(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids]) + ray_starts_w += pos_w.unsqueeze(1) + ray_directions_w = quat_apply(quat_w.repeat(1, self.num_rays), self.ray_directions[env_ids]) + # ray cast and store the hits + # TODO: Make this work for multiple meshes? + self._data.ray_hits_w[env_ids] = raycast_mesh( + ray_starts_w, + ray_directions_w, + max_dist=self.cfg.max_distance, + mesh=self.meshes[self.cfg.mesh_prim_paths[0]], + )[0] + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + if not hasattr(self, "ray_visualizer"): + self.ray_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self.ray_visualizer.set_visibility(True) + else: + if hasattr(self, "ray_visualizer"): + self.ray_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # remove possible inf values + viz_points = self._data.ray_hits_w.reshape(-1, 3) + viz_points = viz_points[~torch.any(torch.isinf(viz_points), dim=1)] + # show ray hit positions + self.ray_visualizer.visualize(viz_points) + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._view = None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera.py new file mode 100644 index 00000000000..004ecad0eea --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera.py @@ -0,0 +1,435 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING, ClassVar, Literal + +import isaacsim.core.utils.stage as stage_utils +import omni.physics.tensors.impl.api as physx +from isaacsim.core.prims import XFormPrim + +import isaaclab.utils.math as math_utils +from isaaclab.sensors.camera import CameraData +from isaaclab.utils.warp import raycast_mesh + +from .ray_caster import RayCaster + +if TYPE_CHECKING: + from .ray_caster_camera_cfg import RayCasterCameraCfg + + +class RayCasterCamera(RayCaster): + """A ray-casting camera sensor. + + The ray-caster camera uses a set of rays to get the distances to meshes in the scene. The rays are + defined in the sensor's local coordinate frame. The sensor has the same interface as the + :class:`isaaclab.sensors.Camera` that implements the camera class through USD camera prims. + However, this class provides a faster image generation. The sensor converts meshes from the list of + primitive paths provided in the configuration to Warp meshes. The camera then ray-casts against these + Warp meshes only. + + Currently, only the following annotators are supported: + + - ``"distance_to_camera"``: An image containing the distance to camera optical center. + - ``"distance_to_image_plane"``: An image containing distances of 3D points from camera plane along camera's z-axis. + - ``"normals"``: An image containing the local surface normal vectors at each pixel. + + .. note:: + Currently, only static meshes are supported. Extending the warp mesh to support dynamic meshes + is a work in progress. + """ + + cfg: RayCasterCameraCfg + """The configuration parameters.""" + UNSUPPORTED_TYPES: ClassVar[set[str]] = { + "rgb", + "instance_id_segmentation", + "instance_id_segmentation_fast", + "instance_segmentation", + "instance_segmentation_fast", + "semantic_segmentation", + "skeleton_data", + "motion_vectors", + "bounding_box_2d_tight", + "bounding_box_2d_tight_fast", + "bounding_box_2d_loose", + "bounding_box_2d_loose_fast", + "bounding_box_3d", + "bounding_box_3d_fast", + } + """A set of sensor types that are not supported by the ray-caster camera.""" + + def __init__(self, cfg: RayCasterCameraCfg): + """Initializes the camera object. + + Args: + cfg: The configuration parameters. + + Raises: + ValueError: If the provided data types are not supported by the ray-caster camera. + """ + # perform check on supported data types + self._check_supported_data_types(cfg) + # initialize base class + super().__init__(cfg) + # create empty variables for storing output data + self._data = CameraData() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Ray-Caster-Camera @ '{self.cfg.prim_path}': \n" + f"\tview type : {self._view.__class__}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tnumber of meshes : {len(self.meshes)}\n" + f"\tnumber of sensors : {self._view.count}\n" + f"\tnumber of rays/sensor: {self.num_rays}\n" + f"\ttotal number of rays : {self.num_rays * self._view.count}\n" + f"\timage shape : {self.image_shape}" + ) + + """ + Properties + """ + + @property + def data(self) -> CameraData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def image_shape(self) -> tuple[int, int]: + """A tuple containing (height, width) of the camera sensor.""" + return (self.cfg.pattern_cfg.height, self.cfg.pattern_cfg.width) + + @property + def frame(self) -> torch.tensor: + """Frame number when the measurement took place.""" + return self._frame + + """ + Operations. + """ + + def set_intrinsic_matrices( + self, matrices: torch.Tensor, focal_length: float = 1.0, env_ids: Sequence[int] | None = None + ): + """Set the intrinsic matrix of the camera. + + Args: + matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). + focal_length: Focal length to use when computing aperture values (in cm). Defaults to 1.0. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + """ + # resolve env_ids + if env_ids is None: + env_ids = slice(None) + # save new intrinsic matrices and focal length + self._data.intrinsic_matrices[env_ids] = matrices.to(self._device) + self._focal_length = focal_length + # recompute ray directions + self.ray_starts[env_ids], self.ray_directions[env_ids] = self.cfg.pattern_cfg.func( + self.cfg.pattern_cfg, self._data.intrinsic_matrices[env_ids], self._device + ) + + def reset(self, env_ids: Sequence[int] | None = None): + # reset the timestamps + super().reset(env_ids) + # resolve None + if env_ids is None: + env_ids = slice(None) + # reset the data + # note: this recomputation is useful if one performs events such as randomizations on the camera poses. + pos_w, quat_w = self._compute_camera_world_poses(env_ids) + self._data.pos_w[env_ids] = pos_w + self._data.quat_w_world[env_ids] = quat_w + # Reset the frame count + self._frame[env_ids] = 0 + + def set_world_poses( + self, + positions: torch.Tensor | None = None, + orientations: torch.Tensor | None = None, + env_ids: Sequence[int] | None = None, + convention: Literal["opengl", "ros", "world"] = "ros", + ): + """Set the pose of the camera w.r.t. the world frame using specified convention. + + Since different fields use different conventions for camera orientations, the method allows users to + set the camera poses in the specified convention. Possible conventions are: + + - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention + - :obj:`"ros"` - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention + - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention + + See :meth:`isaaclab.utils.maths.convert_camera_frame_orientation_convention` for more details + on the conventions. + + Args: + positions: The cartesian coordinates (in meters). Shape is (N, 3). + Defaults to None, in which case the camera position in not changed. + orientations: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the camera orientation in not changed. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + convention: The convention in which the poses are fed. Defaults to "ros". + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + + # get current positions + pos_w, quat_w = self._compute_view_world_poses(env_ids) + if positions is not None: + # transform to camera frame + pos_offset_world_frame = positions - pos_w + self._offset_pos[env_ids] = math_utils.quat_apply(math_utils.quat_inv(quat_w), pos_offset_world_frame) + if orientations is not None: + # convert rotation matrix from input convention to world + quat_w_set = math_utils.convert_camera_frame_orientation_convention( + orientations, origin=convention, target="world" + ) + self._offset_quat[env_ids] = math_utils.quat_mul(math_utils.quat_inv(quat_w), quat_w_set) + + # update the data + pos_w, quat_w = self._compute_camera_world_poses(env_ids) + self._data.pos_w[env_ids] = pos_w + self._data.quat_w_world[env_ids] = quat_w + + def set_world_poses_from_view( + self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None + ): + """Set the poses of the camera from the eye position and look-at target position. + + Args: + eyes: The positions of the camera's eye. Shape is N, 3). + targets: The target locations to look at. Shape is (N, 3). + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + + Raises: + RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first. + NotImplementedError: If the stage up-axis is not "Y" or "Z". + """ + # get up axis of current stage + up_axis = stage_utils.get_stage_up_axis() + # camera position and rotation in opengl convention + orientations = math_utils.quat_from_matrix( + math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis=up_axis, device=self._device) + ) + self.set_world_poses(eyes, orientations, env_ids, convention="opengl") + + """ + Implementation. + """ + + def _initialize_rays_impl(self): + # Create all indices buffer + self._ALL_INDICES = torch.arange(self._view.count, device=self._device, dtype=torch.long) + # Create frame count buffer + self._frame = torch.zeros(self._view.count, device=self._device, dtype=torch.long) + # create buffers + self._create_buffers() + # compute intrinsic matrices + self._compute_intrinsic_matrices() + # compute ray stars and directions + self.ray_starts, self.ray_directions = self.cfg.pattern_cfg.func( + self.cfg.pattern_cfg, self._data.intrinsic_matrices, self._device + ) + self.num_rays = self.ray_directions.shape[1] + # create buffer to store ray hits + self.ray_hits_w = torch.zeros(self._view.count, self.num_rays, 3, device=self._device) + # set offsets + quat_w = math_utils.convert_camera_frame_orientation_convention( + torch.tensor([self.cfg.offset.rot], device=self._device), origin=self.cfg.offset.convention, target="world" + ) + self._offset_quat = quat_w.repeat(self._view.count, 1) + self._offset_pos = torch.tensor(list(self.cfg.offset.pos), device=self._device).repeat(self._view.count, 1) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data.""" + # increment frame count + self._frame[env_ids] += 1 + + # compute poses from current view + pos_w, quat_w = self._compute_camera_world_poses(env_ids) + # update the data + self._data.pos_w[env_ids] = pos_w + self._data.quat_w_world[env_ids] = quat_w + + # note: full orientation is considered + ray_starts_w = math_utils.quat_apply(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids]) + ray_starts_w += pos_w.unsqueeze(1) + ray_directions_w = math_utils.quat_apply(quat_w.repeat(1, self.num_rays), self.ray_directions[env_ids]) + + # ray cast and store the hits + # note: we set max distance to 1e6 during the ray-casting. THis is because we clip the distance + # to the image plane and distance to the camera to the maximum distance afterwards in-order to + # match the USD camera behavior. + + # TODO: Make ray-casting work for multiple meshes? + # necessary for regular dictionaries. + self.ray_hits_w, ray_depth, ray_normal, _ = raycast_mesh( + ray_starts_w, + ray_directions_w, + mesh=self.meshes[self.cfg.mesh_prim_paths[0]], + max_dist=1e6, + return_distance=any( + [name in self.cfg.data_types for name in ["distance_to_image_plane", "distance_to_camera"]] + ), + return_normal="normals" in self.cfg.data_types, + ) + # update output buffers + if "distance_to_image_plane" in self.cfg.data_types: + # note: data is in camera frame so we only take the first component (z-axis of camera frame) + distance_to_image_plane = ( + math_utils.quat_apply( + math_utils.quat_inv(quat_w).repeat(1, self.num_rays), + (ray_depth[:, :, None] * ray_directions_w), + ) + )[:, :, 0] + # apply the maximum distance after the transformation + if self.cfg.depth_clipping_behavior == "max": + distance_to_image_plane = torch.clip(distance_to_image_plane, max=self.cfg.max_distance) + distance_to_image_plane[torch.isnan(distance_to_image_plane)] = self.cfg.max_distance + elif self.cfg.depth_clipping_behavior == "zero": + distance_to_image_plane[distance_to_image_plane > self.cfg.max_distance] = 0.0 + distance_to_image_plane[torch.isnan(distance_to_image_plane)] = 0.0 + self._data.output["distance_to_image_plane"][env_ids] = distance_to_image_plane.view( + -1, *self.image_shape, 1 + ) + + if "distance_to_camera" in self.cfg.data_types: + if self.cfg.depth_clipping_behavior == "max": + ray_depth = torch.clip(ray_depth, max=self.cfg.max_distance) + elif self.cfg.depth_clipping_behavior == "zero": + ray_depth[ray_depth > self.cfg.max_distance] = 0.0 + self._data.output["distance_to_camera"][env_ids] = ray_depth.view(-1, *self.image_shape, 1) + + if "normals" in self.cfg.data_types: + self._data.output["normals"][env_ids] = ray_normal.view(-1, *self.image_shape, 3) + + def _debug_vis_callback(self, event): + # in case it crashes be safe + if not hasattr(self, "ray_hits_w"): + return + # show ray hit positions + self.ray_visualizer.visualize(self.ray_hits_w.view(-1, 3)) + + """ + Private Helpers + """ + + def _check_supported_data_types(self, cfg: RayCasterCameraCfg): + """Checks if the data types are supported by the ray-caster camera.""" + # check if there is any intersection in unsupported types + # reason: we cannot obtain this data from simplified warp-based ray caster + common_elements = set(cfg.data_types) & RayCasterCamera.UNSUPPORTED_TYPES + if common_elements: + raise ValueError( + f"RayCasterCamera class does not support the following sensor types: {common_elements}." + "\n\tThis is because these sensor types cannot be obtained in a fast way using ''warp''." + "\n\tHint: If you need to work with these sensor types, we recommend using the USD camera" + " interface from the isaaclab.sensors.camera module." + ) + + def _create_buffers(self): + """Create buffers for storing data.""" + # prepare drift + self.drift = torch.zeros(self._view.count, 3, device=self.device) + # create the data object + # -- pose of the cameras + self._data.pos_w = torch.zeros((self._view.count, 3), device=self._device) + self._data.quat_w_world = torch.zeros((self._view.count, 4), device=self._device) + # -- intrinsic matrix + self._data.intrinsic_matrices = torch.zeros((self._view.count, 3, 3), device=self._device) + self._data.intrinsic_matrices[:, 2, 2] = 1.0 + self._data.image_shape = self.image_shape + # -- output data + # create the buffers to store the annotator data. + self._data.output = {} + self._data.info = [{name: None for name in self.cfg.data_types}] * self._view.count + for name in self.cfg.data_types: + if name in ["distance_to_image_plane", "distance_to_camera"]: + shape = (self.cfg.pattern_cfg.height, self.cfg.pattern_cfg.width, 1) + elif name in ["normals"]: + shape = (self.cfg.pattern_cfg.height, self.cfg.pattern_cfg.width, 3) + else: + raise ValueError(f"Received unknown data type: {name}. Please check the configuration.") + # allocate tensor to store the data + self._data.output[name] = torch.zeros((self._view.count, *shape), device=self._device) + + def _compute_intrinsic_matrices(self): + """Computes the intrinsic matrices for the camera based on the config provided.""" + # get the sensor properties + pattern_cfg = self.cfg.pattern_cfg + + # check if vertical aperture is provided + # if not then it is auto-computed based on the aspect ratio to preserve squared pixels + if pattern_cfg.vertical_aperture is None: + pattern_cfg.vertical_aperture = pattern_cfg.horizontal_aperture * pattern_cfg.height / pattern_cfg.width + + # compute the intrinsic matrix + f_x = pattern_cfg.width * pattern_cfg.focal_length / pattern_cfg.horizontal_aperture + f_y = pattern_cfg.height * pattern_cfg.focal_length / pattern_cfg.vertical_aperture + c_x = pattern_cfg.horizontal_aperture_offset * f_x + pattern_cfg.width / 2 + c_y = pattern_cfg.vertical_aperture_offset * f_y + pattern_cfg.height / 2 + # allocate the intrinsic matrices + self._data.intrinsic_matrices[:, 0, 0] = f_x + self._data.intrinsic_matrices[:, 0, 2] = c_x + self._data.intrinsic_matrices[:, 1, 1] = f_y + self._data.intrinsic_matrices[:, 1, 2] = c_y + + # save focal length + self._focal_length = pattern_cfg.focal_length + + def _compute_view_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Tensor, torch.Tensor]: + """Obtains the pose of the view the camera is attached to in the world frame. + + Returns: + A tuple of the position (in meters) and quaternion (w, x, y, z). + """ + # obtain the poses of the sensors + # note: clone arg doesn't exist for xform prim view so we need to do this manually + if isinstance(self._view, XFormPrim): + pos_w, quat_w = self._view.get_world_poses(env_ids) + elif isinstance(self._view, physx.ArticulationView): + pos_w, quat_w = self._view.get_root_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = math_utils.convert_quat(quat_w, to="wxyz") + elif isinstance(self._view, physx.RigidBodyView): + pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1) + quat_w = math_utils.convert_quat(quat_w, to="wxyz") + else: + raise RuntimeError(f"Unsupported view type: {type(self._view)}") + # return the pose + return pos_w.clone(), quat_w.clone() + + def _compute_camera_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Tensor, torch.Tensor]: + """Computes the pose of the camera in the world frame. + + This function applies the offset pose to the pose of the view the camera is attached to. + + Returns: + A tuple of the position (in meters) and quaternion (w, x, y, z) in "world" convention. + """ + # get the pose of the view the camera is attached to + pos_w, quat_w = self._compute_view_world_poses(env_ids) + # apply offsets + # need to apply quat because offset relative to parent frame + pos_w += math_utils.quat_apply(quat_w, self._offset_pos[env_ids]) + quat_w = math_utils.quat_mul(quat_w, self._offset_quat[env_ids]) + + return pos_w, quat_w diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py new file mode 100644 index 00000000000..72454f596f9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_camera_cfg.py @@ -0,0 +1,68 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ray-cast camera sensor.""" + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .patterns import PinholeCameraPatternCfg +from .ray_caster_camera import RayCasterCamera +from .ray_caster_cfg import RayCasterCfg + + +@configclass +class RayCasterCameraCfg(RayCasterCfg): + """Configuration for the ray-cast sensor.""" + + @configclass + class OffsetCfg: + """The offset pose of the sensor's frame from the sensor's parent frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + convention: Literal["opengl", "ros", "world"] = "ros" + """The convention in which the frame offset is applied. Defaults to "ros". + + - ``"opengl"`` - forward axis: ``-Z`` - up axis: ``+Y`` - Offset is applied in the OpenGL (Usd.Camera) convention. + - ``"ros"`` - forward axis: ``+Z`` - up axis: ``-Y`` - Offset is applied in the ROS convention. + - ``"world"`` - forward axis: ``+X`` - up axis: ``+Z`` - Offset is applied in the World Frame convention. + + """ + + class_type: type = RayCasterCamera + + offset: OffsetCfg = OffsetCfg() + """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" + + data_types: list[str] = ["distance_to_image_plane"] + """List of sensor names/types to enable for the camera. Defaults to ["distance_to_image_plane"].""" + + depth_clipping_behavior: Literal["max", "zero", "none"] = "none" + """Clipping behavior for the camera for values exceed the maximum value. Defaults to "none". + + - ``"max"``: Values are clipped to the maximum value. + - ``"zero"``: Values are clipped to zero. + - ``"none``: No clipping is applied. Values will be returned as ``inf`` for ``distance_to_camera`` and ``nan`` + for ``distance_to_image_plane`` data type. + """ + + pattern_cfg: PinholeCameraPatternCfg = MISSING + """The pattern that defines the local ray starting positions and directions in a pinhole camera pattern.""" + + def __post_init__(self): + # for cameras, this quantity should be False always. + self.attach_yaw_only = False diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_cfg.py new file mode 100644 index 00000000000..e2cc633a6b4 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ray-cast sensor.""" + + +from dataclasses import MISSING + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import RAY_CASTER_MARKER_CFG +from isaaclab.utils import configclass + +from ..sensor_base_cfg import SensorBaseCfg +from .patterns.patterns_cfg import PatternBaseCfg +from .ray_caster import RayCaster + + +@configclass +class RayCasterCfg(SensorBaseCfg): + """Configuration for the ray-cast sensor.""" + + @configclass + class OffsetCfg: + """The offset pose of the sensor's frame from the sensor's parent frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + class_type: type = RayCaster + + mesh_prim_paths: list[str] = MISSING + """The list of mesh primitive paths to ray cast against. + + Note: + Currently, only a single static mesh is supported. We are working on supporting multiple + static meshes and dynamic meshes. + """ + + offset: OffsetCfg = OffsetCfg() + """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" + + attach_yaw_only: bool = MISSING + """Whether the rays' starting positions and directions only track the yaw orientation. + + This is useful for ray-casting height maps, where only yaw rotation is needed. + """ + + pattern_cfg: PatternBaseCfg = MISSING + """The pattern that defines the local ray starting positions and directions.""" + + max_distance: float = 1e6 + """Maximum distance (in meters) from the sensor to ray cast to. Defaults to 1e6.""" + + drift_range: tuple[float, float] = (0.0, 0.0) + """The range of drift (in meters) to add to the ray starting positions (xyz). Defaults to (0.0, 0.0). + + For floating base robots, this is useful for simulating drift in the robot's pose estimation. + """ + + visualizer_cfg: VisualizationMarkersCfg = RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/RayCaster") + """The configuration object for the visualization markers. Defaults to RAY_CASTER_MARKER_CFG. + + Note: + This attribute is only used when debug visualization is enabled. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_data.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_data.py new file mode 100644 index 00000000000..b4b55809108 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/ray_caster/ray_caster_data.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class RayCasterData: + """Data container for the ray-cast sensor.""" + + pos_w: torch.Tensor = None + """Position of the sensor origin in world frame. + + Shape is (N, 3), where N is the number of sensors. + """ + quat_w: torch.Tensor = None + """Orientation of the sensor origin in quaternion (w, x, y, z) in world frame. + + Shape is (N, 4), where N is the number of sensors. + """ + ray_hits_w: torch.Tensor = None + """The ray hit positions in the world frame. + + Shape is (N, B, 3), where N is the number of sensors, B is the number of rays + in the scan pattern per sensor. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base.py new file mode 100644 index 00000000000..8c6cbfbe852 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base.py @@ -0,0 +1,336 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base class for sensors. + +This class defines an interface for sensors similar to how the :class:`isaaclab.assets.AssetBase` class works. +Each sensor class should inherit from this class and implement the abstract methods. +""" + +from __future__ import annotations + +import builtins +import inspect +import re +import torch +import weakref +from abc import ABC, abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import omni.kit.app +import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager + +import isaaclab.sim as sim_utils + +if TYPE_CHECKING: + from .sensor_base_cfg import SensorBaseCfg + + +class SensorBase(ABC): + """The base class for implementing a sensor. + + The implementation is based on lazy evaluation. The sensor data is only updated when the user + tries accessing the data through the :attr:`data` property or sets ``force_compute=True`` in + the :meth:`update` method. This is done to avoid unnecessary computation when the sensor data + is not used. + + The sensor is updated at the specified update period. If the update period is zero, then the + sensor is updated at every simulation step. + """ + + def __init__(self, cfg: SensorBaseCfg): + """Initialize the sensor class. + + Args: + cfg: The configuration parameters for the sensor. + """ + # check that config is valid + if cfg.history_length < 0: + raise ValueError(f"History length must be greater than 0! Received: {cfg.history_length}") + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg.copy() + # flag for whether the sensor is initialized + self._is_initialized = False + # flag for whether the sensor is in visualization mode + self._is_visualizing = False + + # note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called. + # add callbacks for stage play/stop + # The order is set to 10 which is arbitrary but should be lower priority than the default order of 0 + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj=weakref.proxy(self): obj._initialize_callback(event), + order=10, + ) + self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj=weakref.proxy(self): obj._invalidate_initialize_callback(event), + order=10, + ) + self._prim_deletion_callback_id = SimulationManager.register_callback( + self._on_prim_deletion, event=IsaacEvents.PRIM_DELETION + ) + # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) + self._debug_vis_handle = None + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + + def __del__(self): + """Unsubscribe from the callbacks.""" + # clear physics events handles + self._clear_callbacks() + + """ + Properties + """ + + @property + def is_initialized(self) -> bool: + """Whether the sensor is initialized. + + Returns True if the sensor is initialized, False otherwise. + """ + return self._is_initialized + + @property + def num_instances(self) -> int: + """Number of instances of the sensor. + + This is equal to the number of sensors per environment multiplied by the number of environments. + """ + return self._num_envs + + @property + def device(self) -> str: + """Memory device for computation.""" + return self._device + + @property + @abstractmethod + def data(self) -> Any: + """Data from the sensor. + + This property is only updated when the user tries to access the data. This is done to avoid + unnecessary computation when the sensor data is not used. + + For updating the sensor when this property is accessed, you can use the following + code snippet in your sensor implementation: + + .. code-block:: python + + # update sensors if needed + self._update_outdated_buffers() + # return the data (where `_data` is the data for the sensor) + return self._data + """ + raise NotImplementedError + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the sensor has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_debug_vis_impl) + return "NotImplementedError" not in source_code + + """ + Operations + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Sets whether to visualize the sensor data. + + Args: + debug_vis: Whether to visualize the sensor data. + + Returns: + Whether the debug visualization was successfully set. False if the sensor + does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_debug_vis_implementation: + return False + # toggle debug visualization objects + self._set_debug_vis_impl(debug_vis) + # toggle debug visualization flag + self._is_visualizing = debug_vis + # toggle debug visualization handles + if debug_vis: + # create a subscriber for the post update event if it doesn't exist + if self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + # return success + return True + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the sensor internals. + + Args: + env_ids: The sensor ids to reset. Defaults to None. + """ + # Resolve sensor ids + if env_ids is None: + env_ids = slice(None) + # Reset the timestamp for the sensors + self._timestamp[env_ids] = 0.0 + self._timestamp_last_update[env_ids] = 0.0 + # Set all reset sensors to outdated so that they are updated when data is called the next time. + self._is_outdated[env_ids] = True + + def update(self, dt: float, force_recompute: bool = False): + # Update the timestamp for the sensors + self._timestamp += dt + self._is_outdated |= self._timestamp - self._timestamp_last_update + 1e-6 >= self.cfg.update_period + # Update the buffers + # TODO (from @mayank): Why is there a history length here when it doesn't mean anything in the sensor base?!? + # It is only for the contact sensor but there we should redefine the update function IMO. + if force_recompute or self._is_visualizing or (self.cfg.history_length > 0): + self._update_outdated_buffers() + + """ + Implementation specific. + """ + + @abstractmethod + def _initialize_impl(self): + """Initializes the sensor-related handles and internal buffers.""" + # Obtain Simulation Context + sim = sim_utils.SimulationContext.instance() + if sim is None: + raise RuntimeError("Simulation Context is not initialized!") + # Obtain device and backend + self._device = sim.device + self._backend = sim.backend + self._sim_physics_dt = sim.get_physics_dt() + # Count number of environments + env_prim_path_expr = self.cfg.prim_path.rsplit("/", 1)[0] + self._parent_prims = sim_utils.find_matching_prims(env_prim_path_expr) + self._num_envs = len(self._parent_prims) + # Boolean tensor indicating whether the sensor data has to be refreshed + self._is_outdated = torch.ones(self._num_envs, dtype=torch.bool, device=self._device) + # Current timestamp (in seconds) + self._timestamp = torch.zeros(self._num_envs, device=self._device) + # Timestamp from last update + self._timestamp_last_update = torch.zeros_like(self._timestamp) + + @abstractmethod + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the sensor data for provided environment ids. + + This function does not perform any time-based checks and directly fills the data into the + data container. + + Args: + env_ids: The indices of the sensors that are ready to capture. + """ + raise NotImplementedError + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + def _debug_vis_callback(self, event): + """Callback for debug visualization. + + This function calls the visualization objects and sets the data to visualize into them. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + """ + Internal simulation callbacks. + """ + + def _initialize_callback(self, event): + """Initializes the scene elements. + + Note: + PhysX handles are only enabled once the simulator starts playing. Hence, this function needs to be + called whenever the simulator "plays" from a "stop" state. + """ + if not self._is_initialized: + try: + self._initialize_impl() + except Exception as e: + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e + self._is_initialized = True + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + self._is_initialized = False + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + def _on_prim_deletion(self, prim_path: str) -> None: + """Invalidates and deletes the callbacks when the prim is deleted. + + Args: + prim_path: The path to the prim that is being deleted. + + Note: + This function is called when the prim is deleted. + """ + if prim_path == "/": + self._clear_callbacks() + return + result = re.match( + pattern="^" + "/".join(self.cfg.prim_path.split("/")[: prim_path.count("/") + 1]) + "$", string=prim_path + ) + if result: + self._clear_callbacks() + + def _clear_callbacks(self) -> None: + """Clears the callbacks.""" + if self._prim_deletion_callback_id: + SimulationManager.deregister_callback(self._prim_deletion_callback_id) + self._prim_deletion_callback_id = None + if self._initialize_handle: + self._initialize_handle.unsubscribe() + self._initialize_handle = None + if self._invalidate_initialize_handle: + self._invalidate_initialize_handle.unsubscribe() + self._invalidate_initialize_handle = None + # clear debug visualization + if self._debug_vis_handle: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + """ + Helper functions. + """ + + def _update_outdated_buffers(self): + """Fills the sensor data for the outdated sensors.""" + outdated_env_ids = self._is_outdated.nonzero().squeeze(-1) + if len(outdated_env_ids) > 0: + # obtain new data + self._update_buffers_impl(outdated_env_ids) + # update the timestamp from last update + self._timestamp_last_update[outdated_env_ids] = self._timestamp[outdated_env_ids] + # set outdated flag to false for the updated sensors + self._is_outdated[outdated_env_ids] = False diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base_cfg.py new file mode 100644 index 00000000000..256fff61688 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sensors/sensor_base_cfg.py @@ -0,0 +1,47 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .sensor_base import SensorBase + + +@configclass +class SensorBaseCfg: + """Configuration parameters for a sensor.""" + + class_type: type[SensorBase] = MISSING + """The associated sensor class. + + The class should inherit from :class:`isaaclab.sensors.sensor_base.SensorBase`. + """ + + prim_path: str = MISSING + """Prim path (or expression) to the sensor. + + .. note:: + The expression can contain the environment namespace regex ``{ENV_REGEX_NS}`` which + will be replaced with the environment namespace. + + Example: ``{ENV_REGEX_NS}/Robot/sensor`` will be replaced with ``/World/envs/env_.*/Robot/sensor``. + + """ + + update_period: float = 0.0 + """Update period of the sensor buffers (in seconds). Defaults to 0.0 (update every step).""" + + history_length: int = 0 + """Number of past frames to store in the sensor buffers. Defaults to 0, which means that only + the current data is stored (no history).""" + + debug_vis: bool = False + """Whether to visualize the sensor. Defaults to False.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/__init__.py new file mode 100644 index 00000000000..10571cdd629 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/__init__.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package containing simulation-specific functionalities. + +These include: + +* Ability to spawn different objects and materials into Omniverse +* Define and modify various schemas on USD prims +* Converters to obtain USD file from other file formats (such as URDF, OBJ, STL, FBX) +* Utility class to control the simulator + +.. note:: + Currently, only a subset of all possible schemas and prims in Omniverse are supported. + We are expanding the these set of functions on a need basis. In case, there are + specific prims or schemas that you would like to include, please open an issue on GitHub + as a feature request elaborating on the required application. + +To make it convenient to use the module, we recommend importing the module as follows: + +.. code-block:: python + + import isaaclab.sim as sim_utils + +""" + +from .converters import * # noqa: F401, F403 +from .schemas import * # noqa: F401, F403 +from .simulation_cfg import PhysxCfg, RenderCfg, SimulationCfg # noqa: F401, F403 +from .simulation_context import SimulationContext, build_simulation_context # noqa: F401, F403 +from .spawners import * # noqa: F401, F403 +from .utils import * # noqa: F401, F403 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/__init__.py new file mode 100644 index 00000000000..528fa62e80e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/__init__.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing converters for converting various file types to USD. + +In order to support direct loading of various file types into Omniverse, we provide a set of +converters that can convert the file into a USD file. The converters are implemented as +sub-classes of the :class:`AssetConverterBase` class. + +The following converters are currently supported: + +* :class:`UrdfConverter`: Converts a URDF file into a USD file. +* :class:`MeshConverter`: Converts a mesh file into a USD file. This supports OBJ, STL and FBX files. + +""" + +from .asset_converter_base import AssetConverterBase +from .asset_converter_base_cfg import AssetConverterBaseCfg +from .mesh_converter import MeshConverter +from .mesh_converter_cfg import MeshConverterCfg +from .mjcf_converter import MjcfConverter +from .mjcf_converter_cfg import MjcfConverterCfg +from .urdf_converter import UrdfConverter +from .urdf_converter_cfg import UrdfConverterCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base.py new file mode 100644 index 00000000000..6ec1c23b43f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base.py @@ -0,0 +1,203 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import abc +import hashlib +import json +import os +import pathlib +import random +from datetime import datetime + +from isaaclab.sim.converters.asset_converter_base_cfg import AssetConverterBaseCfg +from isaaclab.utils.assets import check_file_path +from isaaclab.utils.io import dump_yaml + + +class AssetConverterBase(abc.ABC): + """Base class for converting an asset file from different formats into USD format. + + This class provides a common interface for converting an asset file into USD. It does not + provide any implementation for the conversion. The derived classes must implement the + :meth:`_convert_asset` method to provide the actual conversion. + + The file conversion is lazy if the output directory (:obj:`AssetConverterBaseCfg.usd_dir`) is provided. + In the lazy conversion, the USD file is re-generated only if: + + * The asset file is modified. + * The configuration parameters are modified. + * The USD file does not exist. + + To override this behavior to force conversion, the flag :obj:`AssetConverterBaseCfg.force_usd_conversion` + can be set to True. + + When no output directory is defined, lazy conversion is deactivated and the generated USD file is + stored in folder ``/tmp/IsaacLab/usd_{date}_{time}_{random}``, where the parameters in braces are generated + at runtime. The random identifiers help avoid a race condition where two simultaneously triggered conversions + try to use the same directory for reading/writing the generated files. + + .. note:: + Changes to the parameters :obj:`AssetConverterBaseCfg.asset_path`, :obj:`AssetConverterBaseCfg.usd_dir`, and + :obj:`AssetConverterBaseCfg.usd_file_name` are not considered as modifications in the configuration instance that + trigger USD file re-generation. + + """ + + def __init__(self, cfg: AssetConverterBaseCfg): + """Initializes the class. + + Args: + cfg: The configuration instance for converting an asset file to USD format. + + Raises: + ValueError: When provided asset file does not exist. + """ + # check that the config is valid + cfg.validate() + # check if the asset file exists + if not check_file_path(cfg.asset_path): + raise ValueError(f"The asset path does not exist: {cfg.asset_path}") + # save the inputs + self.cfg = cfg + + # resolve USD directory name + if cfg.usd_dir is None: + # a folder in "/tmp/IsaacLab" by the name: usd_{date}_{time}_{random} + time_tag = datetime.now().strftime("%Y%m%d_%H%M%S") + self._usd_dir = f"/tmp/IsaacLab/usd_{time_tag}_{random.randrange(10000)}" + else: + self._usd_dir = cfg.usd_dir + + # resolve the file name from asset file name if not provided + if cfg.usd_file_name is None: + usd_file_name = pathlib.PurePath(cfg.asset_path).stem + else: + usd_file_name = cfg.usd_file_name + # add USD extension if not provided + if not (usd_file_name.endswith(".usd") or usd_file_name.endswith(".usda")): + self._usd_file_name = usd_file_name + ".usd" + else: + self._usd_file_name = usd_file_name + + # create the USD directory + os.makedirs(self.usd_dir, exist_ok=True) + # check if usd files exist + self._usd_file_exists = os.path.isfile(self.usd_path) + # path to read/write asset hash file + self._dest_hash_path = os.path.join(self.usd_dir, ".asset_hash") + # create asset hash to check if the asset has changed + self._asset_hash = self._config_to_hash(cfg) + # read the saved hash + try: + with open(self._dest_hash_path) as f: + existing_asset_hash = f.readline() + self._is_same_asset = existing_asset_hash == self._asset_hash + except FileNotFoundError: + self._is_same_asset = False + + # convert the asset to USD if the hash is different or USD file does not exist + if cfg.force_usd_conversion or not self._usd_file_exists or not self._is_same_asset: + # write the updated hash + with open(self._dest_hash_path, "w") as f: + f.write(self._asset_hash) + # convert the asset to USD + self._convert_asset(cfg) + # dump the configuration to a file + dump_yaml(os.path.join(self.usd_dir, "config.yaml"), cfg.to_dict()) + # add comment to top of the saved config file with information about the converter + current_date = datetime.now().strftime("%Y-%m-%d") + current_time = datetime.now().strftime("%H:%M:%S") + generation_comment = ( + f"##\n# Generated by {self.__class__.__name__} on {current_date} at {current_time}.\n##\n" + ) + with open(os.path.join(self.usd_dir, "config.yaml"), "a") as f: + f.write(generation_comment) + + """ + Properties. + """ + + @property + def usd_dir(self) -> str: + """The absolute path to the directory where the generated USD files are stored.""" + return self._usd_dir + + @property + def usd_file_name(self) -> str: + """The file name of the generated USD file.""" + return self._usd_file_name + + @property + def usd_path(self) -> str: + """The absolute path to the generated USD file.""" + return os.path.join(self.usd_dir, self.usd_file_name) + + @property + def usd_instanceable_meshes_path(self) -> str: + """The relative path to the USD file with meshes. + + The path is with respect to the USD directory :attr:`usd_dir`. This is to ensure that the + mesh references in the generated USD file are resolved relatively. Otherwise, it becomes + difficult to move the USD asset to a different location. + """ + return os.path.join(".", "Props", "instanceable_meshes.usd") + + """ + Implementation specifics. + """ + + @abc.abstractmethod + def _convert_asset(self, cfg: AssetConverterBaseCfg): + """Converts the asset file to USD. + + Args: + cfg: The configuration instance for the input asset to USD conversion. + """ + raise NotImplementedError() + + """ + Private helpers. + """ + + @staticmethod + def _config_to_hash(cfg: AssetConverterBaseCfg) -> str: + """Converts the configuration object and asset file to an MD5 hash of a string. + + .. warning:: + It only checks the main asset file (:attr:`cfg.asset_path`). + + Args: + config : The asset converter configuration object. + + Returns: + An MD5 hash of a string. + """ + + # convert to dict and remove path related info + config_dic = cfg.to_dict() + _ = config_dic.pop("asset_path") + _ = config_dic.pop("usd_dir") + _ = config_dic.pop("usd_file_name") + # convert config dic to bytes + config_bytes = json.dumps(config_dic).encode() + # hash config + md5 = hashlib.md5() + md5.update(config_bytes) + + # read the asset file to observe changes + with open(cfg.asset_path, "rb") as f: + while True: + # read 64kb chunks to avoid memory issues for the large files! + data = f.read(65536) + if not data: + break + md5.update(data) + # return the hash + return md5.hexdigest() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base_cfg.py new file mode 100644 index 00000000000..62605ff207e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/asset_converter_base_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class AssetConverterBaseCfg: + """The base configuration class for asset converters.""" + + asset_path: str = MISSING + """The absolute path to the asset file to convert into USD.""" + + usd_dir: str | None = None + """The output directory path to store the generated USD file. Defaults to None. + + If None, it is resolved as ``/tmp/IsaacLab/usd_{date}_{time}_{random}``, where + the parameters in braces are runtime generated. + """ + + usd_file_name: str | None = None + """The name of the generated usd file. Defaults to None. + + If None, it is resolved from the asset file name. For example, if the asset file + name is ``"my_asset.urdf"``, then the generated USD file name is ``"my_asset.usd"``. + + If the providing file name does not end with ".usd" or ".usda", then the extension + ".usd" is appended to the file name. + """ + + force_usd_conversion: bool = False + """Force the conversion of the asset file to usd. Defaults to False. + + If True, then the USD file is always generated. It will overwrite the existing USD file if it exists. + """ + + make_instanceable: bool = True + """Make the generated USD file instanceable. Defaults to True. + + Note: + Instancing helps reduce the memory footprint of the asset when multiple copies of the asset are + used in the scene. For more information, please check the USD documentation on + `scene-graph instancing `_. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter.py new file mode 100644 index 00000000000..d0b78916cb1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter.py @@ -0,0 +1,250 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import asyncio +import os + +import omni +import omni.kit.commands +import omni.usd +from isaacsim.core.utils.extensions import enable_extension +from pxr import Gf, Tf, Usd, UsdGeom, UsdPhysics, UsdUtils + +from isaaclab.sim.converters.asset_converter_base import AssetConverterBase +from isaaclab.sim.converters.mesh_converter_cfg import MeshConverterCfg +from isaaclab.sim.schemas import schemas +from isaaclab.sim.utils import export_prim_to_file + + +class MeshConverter(AssetConverterBase): + """Converter for a mesh file in OBJ / STL / FBX format to a USD file. + + This class wraps around the `omni.kit.asset_converter`_ extension to provide a lazy implementation + for mesh to USD conversion. It stores the output USD file in an instanceable format since that is + what is typically used in all learning related applications. + + To make the asset instanceable, we must follow a certain structure dictated by how USD scene-graph + instancing and physics work. The rigid body component must be added to each instance and not the + referenced asset (i.e. the prototype prim itself). This is because the rigid body component defines + properties that are specific to each instance and cannot be shared under the referenced asset. For + more information, please check the `documentation `_. + + Due to the above, we follow the following structure: + + * ``{prim_path}`` - The root prim that is an Xform with the rigid body and mass APIs if configured. + * ``{prim_path}/geometry`` - The prim that contains the mesh and optionally the materials if configured. + If instancing is enabled, this prim will be an instanceable reference to the prototype prim. + + .. _omni.kit.asset_converter: https://docs.omniverse.nvidia.com/extensions/latest/ext_asset-converter.html + + .. caution:: + When converting STL files, Z-up convention is assumed, even though this is not the default for many CAD + export programs. Asset orientation convention can either be modified directly in the CAD program's export + process or an offset can be added within the config in Isaac Lab. + + """ + + cfg: MeshConverterCfg + """The configuration instance for mesh to USD conversion.""" + + def __init__(self, cfg: MeshConverterCfg): + """Initializes the class. + + Args: + cfg: The configuration instance for mesh to USD conversion. + """ + super().__init__(cfg=cfg) + + """ + Implementation specific methods. + """ + + def _convert_asset(self, cfg: MeshConverterCfg): + """Generate USD from OBJ, STL or FBX. + + The USD file has Y-up axis and is scaled to meters. + The asset hierarchy is arranged as follows: + + .. code-block:: none + mesh_file_basename (default prim) + |- /geometry/Looks + |- /geometry/mesh + + Args: + cfg: The configuration for conversion of mesh to USD. + + Raises: + RuntimeError: If the conversion using the Omniverse asset converter fails. + """ + # resolve mesh name and format + mesh_file_basename, mesh_file_format = os.path.basename(cfg.asset_path).split(".") + mesh_file_format = mesh_file_format.lower() + + # Check if mesh_file_basename is a valid USD identifier + if not Tf.IsValidIdentifier(mesh_file_basename): + # Correct the name to a valid identifier and update the basename + mesh_file_basename_original = mesh_file_basename + mesh_file_basename = Tf.MakeValidIdentifier(mesh_file_basename) + omni.log.warn( + f"Input file name '{mesh_file_basename_original}' is an invalid identifier for the mesh prim path." + f" Renaming it to '{mesh_file_basename}' for the conversion." + ) + + # Convert USD + asyncio.get_event_loop().run_until_complete( + self._convert_mesh_to_usd(in_file=cfg.asset_path, out_file=self.usd_path) + ) + # Create a new stage, set Z up and meters per unit + temp_stage = Usd.Stage.CreateInMemory() + UsdGeom.SetStageUpAxis(temp_stage, UsdGeom.Tokens.z) + UsdGeom.SetStageMetersPerUnit(temp_stage, 1.0) + UsdPhysics.SetStageKilogramsPerUnit(temp_stage, 1.0) + # Add mesh to stage + base_prim = temp_stage.DefinePrim(f"/{mesh_file_basename}", "Xform") + prim = temp_stage.DefinePrim(f"/{mesh_file_basename}/geometry", "Xform") + prim.GetReferences().AddReference(self.usd_path) + temp_stage.SetDefaultPrim(base_prim) + temp_stage.Export(self.usd_path) + + # Open converted USD stage + stage = Usd.Stage.Open(self.usd_path) + # Need to reload the stage to get the new prim structure, otherwise it can be taken from the cache + stage.Reload() + # Add USD to stage cache + stage_id = UsdUtils.StageCache.Get().Insert(stage) + # Get the default prim (which is the root prim) -- "/{mesh_file_basename}" + xform_prim = stage.GetDefaultPrim() + geom_prim = stage.GetPrimAtPath(f"/{mesh_file_basename}/geometry") + # Move all meshes to underneath new Xform + for child_mesh_prim in geom_prim.GetChildren(): + if child_mesh_prim.GetTypeName() == "Mesh": + # Apply collider properties to mesh + if cfg.collision_props is not None: + # -- Collision approximation to mesh + # TODO: Move this to a new Schema: https://github.com/isaac-orbit/IsaacLab/issues/163 + mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(child_mesh_prim) + mesh_collision_api.GetApproximationAttr().Set(cfg.collision_approximation) + # -- Collider properties such as offset, scale, etc. + schemas.define_collision_properties( + prim_path=child_mesh_prim.GetPath(), cfg=cfg.collision_props, stage=stage + ) + # Delete the old Xform and make the new Xform the default prim + stage.SetDefaultPrim(xform_prim) + # Apply default Xform rotation to mesh -> enable to set rotation and scale + omni.kit.commands.execute( + "CreateDefaultXformOnPrimCommand", + prim_path=xform_prim.GetPath(), + **{"stage": stage}, + ) + + # Apply translation, rotation, and scale to the Xform + geom_xform = UsdGeom.Xform(geom_prim) + geom_xform.ClearXformOpOrder() + + # Remove any existing rotation attributes + rotate_attr = geom_prim.GetAttribute("xformOp:rotateXYZ") + if rotate_attr: + geom_prim.RemoveProperty(rotate_attr.GetName()) + + # translation + translate_op = geom_xform.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(*cfg.translation)) + # rotation + orient_op = geom_xform.AddOrientOp(UsdGeom.XformOp.PrecisionDouble) + orient_op.Set(Gf.Quatd(*cfg.rotation)) + # scale + scale_op = geom_xform.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(*cfg.scale)) + + # Handle instanceable + # Create a new Xform prim that will be the prototype prim + if cfg.make_instanceable: + # Export Xform to a file so we can reference it from all instances + export_prim_to_file( + path=os.path.join(self.usd_dir, self.usd_instanceable_meshes_path), + source_prim_path=geom_prim.GetPath(), + stage=stage, + ) + # Delete the original prim that will now be a reference + geom_prim_path = geom_prim.GetPath().pathString + omni.kit.commands.execute("DeletePrims", paths=[geom_prim_path], stage=stage) + # Update references to exported Xform and make it instanceable + geom_undef_prim = stage.DefinePrim(geom_prim_path) + geom_undef_prim.GetReferences().AddReference(self.usd_instanceable_meshes_path, primPath=geom_prim_path) + geom_undef_prim.SetInstanceable(True) + + # Apply mass and rigid body properties after everything else + # Properties are applied to the top level prim to avoid the case where all instances of this + # asset unintentionally share the same rigid body properties + # apply mass properties + if cfg.mass_props is not None: + schemas.define_mass_properties(prim_path=xform_prim.GetPath(), cfg=cfg.mass_props, stage=stage) + # apply rigid body properties + if cfg.rigid_props is not None: + schemas.define_rigid_body_properties(prim_path=xform_prim.GetPath(), cfg=cfg.rigid_props, stage=stage) + + # Save changes to USD stage + stage.Save() + if stage_id is not None: + UsdUtils.StageCache.Get().Erase(stage_id) + + """ + Helper methods. + """ + + @staticmethod + async def _convert_mesh_to_usd(in_file: str, out_file: str, load_materials: bool = True) -> bool: + """Convert mesh from supported file types to USD. + + This function uses the Omniverse Asset Converter extension to convert a mesh file to USD. + It is an asynchronous function and should be called using `asyncio.get_event_loop().run_until_complete()`. + + The converted asset is stored in the USD format in the specified output file. + The USD file has Y-up axis and is scaled to cm. + + Args: + in_file: The file to convert. + out_file: The path to store the output file. + load_materials: Set to True to enable attaching materials defined in the input file + to the generated USD mesh. Defaults to True. + + Returns: + True if the conversion succeeds. + """ + enable_extension("omni.kit.asset_converter") + + import omni.kit.asset_converter + import omni.usd + + # Create converter context + converter_context = omni.kit.asset_converter.AssetConverterContext() + # Set up converter settings + # Don't import/export materials + converter_context.ignore_materials = not load_materials + converter_context.ignore_animations = True + converter_context.ignore_camera = True + converter_context.ignore_light = True + # Merge all meshes into one + converter_context.merge_all_meshes = True + # Sets world units to meters, this will also scale asset if it's centimeters model. + # This does not work right now :(, so we need to scale the mesh manually + converter_context.use_meter_as_world_unit = True + converter_context.baking_scales = True + # Uses double precision for all transform ops. + converter_context.use_double_precision_to_usd_transform_op = True + + # Create converter task + instance = omni.kit.asset_converter.get_instance() + task = instance.create_converter_task(in_file, out_file, None, converter_context) + # Start conversion task and wait for it to finish + success = await task.wait_until_finished() + if not success: + raise RuntimeError(f"Failed to convert {in_file} to USD. Error: {task.get_error_message()}") + return success diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter_cfg.py new file mode 100644 index 00000000000..cd97b544a70 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mesh_converter_cfg.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.sim.converters.asset_converter_base_cfg import AssetConverterBaseCfg +from isaaclab.sim.schemas import schemas_cfg +from isaaclab.utils import configclass + + +@configclass +class MeshConverterCfg(AssetConverterBaseCfg): + """The configuration class for MeshConverter.""" + + mass_props: schemas_cfg.MassPropertiesCfg | None = None + """Mass properties to apply to the USD. Defaults to None. + + Note: + If None, then no mass properties will be added. + """ + + rigid_props: schemas_cfg.RigidBodyPropertiesCfg | None = None + """Rigid body properties to apply to the USD. Defaults to None. + + Note: + If None, then no rigid body properties will be added. + """ + + collision_props: schemas_cfg.CollisionPropertiesCfg | None = None + """Collision properties to apply to the USD. Defaults to None. + + Note: + If None, then no collision properties will be added. + """ + + collision_approximation: str = "convexDecomposition" + """Collision approximation method to use. Defaults to "convexDecomposition". + + Valid options are: + "convexDecomposition", "convexHull", "boundingCube", + "boundingSphere", "meshSimplification", or "none" + + "none" causes no collision mesh to be added. + """ + + translation: tuple[float, float, float] = (0.0, 0.0, 0.0) + """The translation of the mesh to the origin. Defaults to (0.0, 0.0, 0.0).""" + + rotation: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """The rotation of the mesh in quaternion format (w, x, y, z). Defaults to (1.0, 0.0, 0.0, 0.0).""" + + scale: tuple[float, float, float] = (1.0, 1.0, 1.0) + """The scale of the mesh. Defaults to (1.0, 1.0, 1.0).""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter.py new file mode 100644 index 00000000000..758be1a8e35 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter.py @@ -0,0 +1,108 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import os + +import isaacsim +import omni.kit.commands +import omni.usd + +from .asset_converter_base import AssetConverterBase +from .mjcf_converter_cfg import MjcfConverterCfg + + +class MjcfConverter(AssetConverterBase): + """Converter for a MJCF description file to a USD file. + + This class wraps around the `isaacsim.asset.importer.mjcf`_ extension to provide a lazy implementation + for MJCF to USD conversion. It stores the output USD file in an instanceable format since that is + what is typically used in all learning related applications. + + .. caution:: + The current lazy conversion implementation does not automatically trigger USD generation if + only the mesh files used by the MJCF are modified. To force generation, either set + :obj:`AssetConverterBaseCfg.force_usd_conversion` to True or delete the output directory. + + .. note:: + From Isaac Sim 4.5 onwards, the extension name changed from ``omni.importer.mjcf`` to + ``isaacsim.asset.importer.mjcf``. This converter class now uses the latest extension from Isaac Sim. + + .. _isaacsim.asset.importer.mjcf: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_mjcf.html + """ + + cfg: MjcfConverterCfg + """The configuration instance for MJCF to USD conversion.""" + + def __init__(self, cfg: MjcfConverterCfg): + """Initializes the class. + + Args: + cfg: The configuration instance for URDF to USD conversion. + """ + super().__init__(cfg=cfg) + + """ + Implementation specific methods. + """ + + def _convert_asset(self, cfg: MjcfConverterCfg): + """Calls underlying Omniverse command to convert MJCF to USD. + + Args: + cfg: The configuration instance for MJCF to USD conversion. + """ + import_config = self._get_mjcf_import_config() + file_basename, _ = os.path.basename(cfg.asset_path).split(".") + omni.kit.commands.execute( + "MJCFCreateAsset", + mjcf_path=cfg.asset_path, + import_config=import_config, + dest_path=self.usd_path, + prim_path=f"/{file_basename}", + ) + + def _get_mjcf_import_config(self) -> isaacsim.asset.importer.mjcf.ImportConfig: + """Returns the import configuration for MJCF to USD conversion. + + Returns: + The constructed ``ImportConfig`` object containing the desired settings. + """ + + _, import_config = omni.kit.commands.execute("MJCFCreateImportConfig") + + # set the unit scaling factor, 1.0 means meters, 100.0 means cm + # import_config.set_distance_scale(1.0) + # set imported robot as default prim + # import_config.set_make_default_prim(True) + # add a physics scene to the stage on import if none exists + # import_config.set_create_physics_scene(False) + # set flag to parse tag + import_config.set_import_sites(True) + + # -- instancing settings + # meshes will be placed in a separate usd file + import_config.set_make_instanceable(self.cfg.make_instanceable) + import_config.set_instanceable_usd_path(self.usd_instanceable_meshes_path) + + # -- asset settings + # default density used for links, use 0 to auto-compute + import_config.set_density(self.cfg.link_density) + # import inertia tensor from urdf, if it is not specified in urdf it will import as identity + import_config.set_import_inertia_tensor(self.cfg.import_inertia_tensor) + + # -- physics settings + # create fix joint for base link + import_config.set_fix_base(self.cfg.fix_base) + # self collisions between links in the articulation + import_config.set_self_collision(self.cfg.self_collision) + + return import_config diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter_cfg.py new file mode 100644 index 00000000000..cefd8333c99 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/mjcf_converter_cfg.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.sim.converters.asset_converter_base_cfg import AssetConverterBaseCfg +from isaaclab.utils import configclass + + +@configclass +class MjcfConverterCfg(AssetConverterBaseCfg): + """The configuration class for MjcfConverter.""" + + link_density = 0.0 + """Default density used for links. Defaults to 0. + + This setting is only effective if ``"inertial"`` properties are missing in the MJCF. + """ + + import_inertia_tensor: bool = True + """Import the inertia tensor from mjcf. Defaults to True. + + If the ``"inertial"`` tag is missing, then it is imported as an identity. + """ + + fix_base: bool = MISSING + """Create a fix joint to the root/base link. Defaults to True.""" + + import_sites: bool = True + """Import the sites from the MJCF. Defaults to True.""" + + self_collision: bool = False + """Activate self-collisions between links of the articulation. Defaults to False.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter.py new file mode 100644 index 00000000000..7a253e8a376 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter.py @@ -0,0 +1,327 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import re + +import isaacsim +import omni.kit.app +import omni.kit.commands +import omni.usd +from isaacsim.core.utils.extensions import enable_extension + +from .asset_converter_base import AssetConverterBase +from .urdf_converter_cfg import UrdfConverterCfg + + +class UrdfConverter(AssetConverterBase): + """Converter for a URDF description file to a USD file. + + This class wraps around the `isaacsim.asset.importer.urdf`_ extension to provide a lazy implementation + for URDF to USD conversion. It stores the output USD file in an instanceable format since that is + what is typically used in all learning related applications. + + .. caution:: + The current lazy conversion implementation does not automatically trigger USD generation if + only the mesh files used by the URDF are modified. To force generation, either set + :obj:`AssetConverterBaseCfg.force_usd_conversion` to True or delete the output directory. + + .. note:: + From Isaac Sim 4.5 onwards, the extension name changed from ``omni.importer.urdf`` to + ``isaacsim.asset.importer.urdf``. This converter class now uses the latest extension from Isaac Sim. + + .. _isaacsim.asset.importer.urdf: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_urdf.html + """ + + cfg: UrdfConverterCfg + """The configuration instance for URDF to USD conversion.""" + + def __init__(self, cfg: UrdfConverterCfg): + """Initializes the class. + + Args: + cfg: The configuration instance for URDF to USD conversion. + """ + manager = omni.kit.app.get_app().get_extension_manager() + if not manager.is_extension_enabled("isaacsim.asset.importer.urdf"): + enable_extension("isaacsim.asset.importer.urdf") + from isaacsim.asset.importer.urdf._urdf import acquire_urdf_interface + + self._urdf_interface = acquire_urdf_interface() + super().__init__(cfg=cfg) + + """ + Implementation specific methods. + """ + + def _convert_asset(self, cfg: UrdfConverterCfg): + """Calls underlying Omniverse command to convert URDF to USD. + + Args: + cfg: The URDF conversion configuration. + """ + + import_config = self._get_urdf_import_config() + # parse URDF file + result, self._robot_model = omni.kit.commands.execute( + "URDFParseFile", urdf_path=cfg.asset_path, import_config=import_config + ) + + if result: + if cfg.joint_drive: + # modify joint parameters + self._update_joint_parameters() + + # set root link name + if cfg.root_link_name: + self._robot_model.root_link = cfg.root_link_name + + # convert the model to USD + omni.kit.commands.execute( + "URDFImportRobot", + urdf_path=cfg.asset_path, + urdf_robot=self._robot_model, + import_config=import_config, + dest_path=self.usd_path, + ) + else: + raise ValueError(f"Failed to parse URDF file: {cfg.asset_path}") + + """ + Helper methods. + """ + + def _get_urdf_import_config(self) -> isaacsim.asset.importer.urdf.ImportConfig: + """Create and fill URDF ImportConfig with desired settings + + Returns: + The constructed ``ImportConfig`` object containing the desired settings. + """ + # create a new import config + _, import_config = omni.kit.commands.execute("URDFCreateImportConfig") + + # set the unit scaling factor, 1.0 means meters, 100.0 means cm + import_config.set_distance_scale(1.0) + # set imported robot as default prim + import_config.set_make_default_prim(True) + # add a physics scene to the stage on import if none exists + import_config.set_create_physics_scene(False) + + # -- asset settings + # default density used for links, use 0 to auto-compute + import_config.set_density(self.cfg.link_density) + # mesh simplification settings + convex_decomp = self.cfg.collider_type == "convex_decomposition" + import_config.set_convex_decomp(convex_decomp) + # create collision geometry from visual geometry + import_config.set_collision_from_visuals(self.cfg.collision_from_visuals) + # consolidating links that are connected by fixed joints + import_config.set_merge_fixed_joints(self.cfg.merge_fixed_joints) + # -- physics settings + # create fix joint for base link + import_config.set_fix_base(self.cfg.fix_base) + # self collisions between links in the articulation + import_config.set_self_collision(self.cfg.self_collision) + # convert mimic joints to normal joints + import_config.set_parse_mimic(self.cfg.convert_mimic_joints_to_normal_joints) + # replace cylinder shapes with capsule shapes + import_config.set_replace_cylinders_with_capsules(self.cfg.replace_cylinders_with_capsules) + + return import_config + + def _update_joint_parameters(self): + """Update the joint parameters based on the configuration.""" + # set the drive type + self._set_joints_drive_type() + # set the drive target type + self._set_joints_drive_target_type() + # set the drive gains + self._set_joint_drive_gains() + + def _set_joints_drive_type(self): + """Set the joint drive type for all joints in the URDF model.""" + from isaacsim.asset.importer.urdf._urdf import UrdfJointDriveType + + drive_type_mapping = { + "force": UrdfJointDriveType.JOINT_DRIVE_FORCE, + "acceleration": UrdfJointDriveType.JOINT_DRIVE_ACCELERATION, + } + + if isinstance(self.cfg.joint_drive.drive_type, str): + for joint in self._robot_model.joints.values(): + joint.drive.set_drive_type(drive_type_mapping[self.cfg.joint_drive.drive_type]) + elif isinstance(self.cfg.joint_drive.drive_type, dict): + for joint_name, drive_type in self.cfg.joint_drive.drive_type.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the drive type config was not found in the URDF file. The" + f" joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + joint.drive.set_drive_type(drive_type_mapping[drive_type]) + + def _set_joints_drive_target_type(self): + """Set the joint drive target type for all joints in the URDF model.""" + from isaacsim.asset.importer.urdf._urdf import UrdfJointTargetType + + target_type_mapping = { + "none": UrdfJointTargetType.JOINT_DRIVE_NONE, + "position": UrdfJointTargetType.JOINT_DRIVE_POSITION, + "velocity": UrdfJointTargetType.JOINT_DRIVE_VELOCITY, + } + + if isinstance(self.cfg.joint_drive.target_type, str): + for joint in self._robot_model.joints.values(): + joint.drive.set_target_type(target_type_mapping[self.cfg.joint_drive.target_type]) + elif isinstance(self.cfg.joint_drive.target_type, dict): + for joint_name, target_type in self.cfg.joint_drive.target_type.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the target type config was not found in the URDF file. The" + f" joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + joint.drive.set_target_type(target_type_mapping[target_type]) + + def _set_joint_drive_gains(self): + """Set the joint drive gains for all joints in the URDF model.""" + + # set the gains directly from stiffness and damping values + if isinstance(self.cfg.joint_drive.gains, UrdfConverterCfg.JointDriveCfg.PDGainsCfg): + # stiffness + if isinstance(self.cfg.joint_drive.gains.stiffness, (float, int)): + for joint in self._robot_model.joints.values(): + self._set_joint_drive_stiffness(joint, self.cfg.joint_drive.gains.stiffness) + elif isinstance(self.cfg.joint_drive.gains.stiffness, dict): + for joint_name, stiffness in self.cfg.joint_drive.gains.stiffness.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the drive stiffness config was not found in the URDF file." + f" The joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + self._set_joint_drive_stiffness(joint, stiffness) + # damping + if isinstance(self.cfg.joint_drive.gains.damping, (float, int)): + for joint in self._robot_model.joints.values(): + self._set_joint_drive_damping(joint, self.cfg.joint_drive.gains.damping) + elif isinstance(self.cfg.joint_drive.gains.damping, dict): + for joint_name, damping in self.cfg.joint_drive.gains.damping.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the drive damping config was not found in the URDF file." + f" The joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + self._set_joint_drive_damping(joint, damping) + + # set the gains from natural frequency and damping ratio + elif isinstance(self.cfg.joint_drive.gains, UrdfConverterCfg.JointDriveCfg.NaturalFrequencyGainsCfg): + # damping ratio + if isinstance(self.cfg.joint_drive.gains.damping_ratio, (float, int)): + for joint in self._robot_model.joints.values(): + joint.drive.damping_ratio = self.cfg.joint_drive.gains.damping_ratio + elif isinstance(self.cfg.joint_drive.gains.damping_ratio, dict): + for joint_name, damping_ratio in self.cfg.joint_drive.gains.damping_ratio.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the damping ratio config was not found in the URDF file." + f" The joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + joint.drive.damping_ratio = damping_ratio + + # natural frequency (this has to be done after damping ratio is set) + if isinstance(self.cfg.joint_drive.gains.natural_frequency, (float, int)): + for joint in self._robot_model.joints.values(): + joint.drive.natural_frequency = self.cfg.joint_drive.gains.natural_frequency + self._set_joint_drive_gains_from_natural_frequency(joint) + elif isinstance(self.cfg.joint_drive.gains.natural_frequency, dict): + for joint_name, natural_frequency in self.cfg.joint_drive.gains.natural_frequency.items(): + # handle joint name being a regex + matches = [s for s in self._robot_model.joints.keys() if re.search(joint_name, s)] + if not matches: + raise ValueError( + f"The joint name {joint_name} in the natural frequency config was not found in the URDF" + f" file. The joint names in the URDF are {list(self._robot_model.joints.keys())}" + ) + for match in matches: + joint = self._robot_model.joints[match] + joint.drive.natural_frequency = natural_frequency + self._set_joint_drive_gains_from_natural_frequency(joint) + + def _set_joint_drive_stiffness(self, joint, stiffness: float): + """Set the joint drive stiffness. + + Args: + joint: The joint from the URDF robot model. + stiffness: The stiffness value. + """ + from isaacsim.asset.importer.urdf._urdf import UrdfJointType + + if joint.type == UrdfJointType.JOINT_PRISMATIC: + joint.drive.set_strength(stiffness) + else: + # we need to convert the stiffness from radians to degrees + joint.drive.set_strength(math.pi / 180 * stiffness) + + def _set_joint_drive_damping(self, joint, damping: float): + """Set the joint drive damping. + + Args: + joint: The joint from the URDF robot model. + damping: The damping value. + """ + from isaacsim.asset.importer.urdf._urdf import UrdfJointType + + if joint.type == UrdfJointType.JOINT_PRISMATIC: + joint.drive.set_damping(damping) + else: + # we need to convert the damping from radians to degrees + joint.drive.set_damping(math.pi / 180 * damping) + + def _set_joint_drive_gains_from_natural_frequency(self, joint): + """Compute the joint drive gains from the natural frequency and damping ratio. + + Args: + joint: The joint from the URDF robot model. + """ + from isaacsim.asset.importer.urdf._urdf import UrdfJointDriveType, UrdfJointTargetType + + strength = self._urdf_interface.compute_natural_stiffness( + self._robot_model, + joint.name, + joint.drive.natural_frequency, + ) + self._set_joint_drive_stiffness(joint, strength) + + if joint.drive.target_type == UrdfJointTargetType.JOINT_DRIVE_POSITION: + m_eq = 1.0 + if joint.drive.drive_type == UrdfJointDriveType.JOINT_DRIVE_FORCE: + m_eq = joint.inertia + damping = 2 * m_eq * joint.drive.natural_frequency * joint.drive.damping_ratio + self._set_joint_drive_damping(joint, damping) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter_cfg.py new file mode 100644 index 00000000000..d63ceba4100 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/converters/urdf_converter_cfg.py @@ -0,0 +1,135 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim.converters.asset_converter_base_cfg import AssetConverterBaseCfg +from isaaclab.utils import configclass + + +@configclass +class UrdfConverterCfg(AssetConverterBaseCfg): + """The configuration class for UrdfConverter.""" + + @configclass + class JointDriveCfg: + """Configuration for the joint drive.""" + + @configclass + class PDGainsCfg: + """Configuration for the PD gains of the drive.""" + + stiffness: dict[str, float] | float = MISSING + """The stiffness of the joint drive in Nm/rad or N/rad. + + If None, the stiffness is set to the value parsed from the URDF file. + If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value determines + the drive strength in joint velocity space. + """ + + damping: dict[str, float] | float | None = None + """The damping of the joint drive in Nm/(rad/s) or N/(rad/s). Defaults to None. + + If None, the damping is set to the value parsed from the URDF file or 0.0 if no value is found in the URDF. + If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this attribute is set to + 0.0 and :attr:`stiffness` serves as the drive's strength in joint velocity space. + """ + + @configclass + class NaturalFrequencyGainsCfg: + r"""Configuration for the natural frequency gains of the drive. + + Computes the joint drive stiffness and damping based on the desired natural frequency using the formula: + + :math:`P = m \cdot f^2`, :math:`D = 2 \cdot r \cdot f \cdot m` + + where :math:`f` is the natural frequency, :math:`r` is the damping ratio, and :math:`m` is the total + equivalent inertia at the joint. The damping ratio is such that: + + * :math:`r = 1.0` is a critically damped system, + * :math:`r < 1.0` is underdamped, + * :math:`r > 1.0` is overdamped. + """ + + natural_frequency: dict[str, float] | float = MISSING + """The natural frequency of the joint drive. + + If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value determines the + drive's natural frequency in joint velocity space. + """ + + damping_ratio: dict[str, float] | float = 0.005 + """The damping ratio of the joint drive. Defaults to 0.005. + + If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value is ignored and + only :attr:`natural_frequency` is used. + """ + + drive_type: dict[str, Literal["acceleration", "force"]] | Literal["acceleration", "force"] = "force" + """The drive type used for the joint. Defaults to ``"force"``. + + * ``"acceleration"``: The joint drive normalizes the inertia before applying the joint effort so it's invariant + to inertia and mass changes (equivalent to ideal damped oscillator). + * ``"force"``: Applies effort through forces, so is subject to variations on the body inertia. + """ + + target_type: dict[str, Literal["none", "position", "velocity"]] | Literal["none", "position", "velocity"] = ( + "position" + ) + """The drive target type used for the joint. Defaults to ``"position"``. + + If the target type is set to ``"none"``, the joint stiffness and damping are set to 0.0. + """ + + gains: PDGainsCfg | NaturalFrequencyGainsCfg = PDGainsCfg() + """The drive gains configuration.""" + + fix_base: bool = MISSING + """Create a fix joint to the root/base link.""" + + root_link_name: str | None = None + """The name of the root link. Defaults to None. + + If None, the root link will be set by PhysX. + """ + + link_density: float = 0.0 + """Default density in ``kg/m^3`` for links whose ``"inertial"`` properties are missing in the URDF. Defaults to 0.0.""" + + merge_fixed_joints: bool = True + """Consolidate links that are connected by fixed joints. Defaults to True.""" + + convert_mimic_joints_to_normal_joints: bool = False + """Convert mimic joints to normal joints. Defaults to False.""" + + joint_drive: JointDriveCfg | None = JointDriveCfg() + """The joint drive settings. Defaults to :class:`JointDriveCfg`. + + The parameter can be set to ``None`` for URDFs without joints. + """ + + collision_from_visuals = False + """Whether to create collision geometry from visual geometry. Defaults to False.""" + + collider_type: Literal["convex_hull", "convex_decomposition"] = "convex_hull" + """The collision shape simplification. Defaults to "convex_hull". + + Supported values are: + + * ``"convex_hull"``: The collision shape is simplified to a convex hull. + * ``"convex_decomposition"``: The collision shape is decomposed into smaller convex shapes for a closer fit. + """ + + self_collision: bool = False + """Activate self-collisions between links of the articulation. Defaults to False.""" + + replace_cylinders_with_capsules: bool = False + """Replace cylinder shapes with capsule shapes. Defaults to False.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/__init__.py new file mode 100644 index 00000000000..78c15e30aa0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/__init__.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for schemas used in Omniverse. + +We wrap the USD schemas for PhysX and USD Physics in a more convenient API for setting the parameters from +Python. This is done so that configuration objects can define the schema properties to set and make it easier +to tune the physics parameters without requiring to open Omniverse Kit and manually set the parameters into +the respective USD attributes. + +.. caution:: + + Schema properties cannot be applied on prims that are prototypes as they are read-only prims. This + particularly affects instanced assets where some of the prims (usually the visual and collision meshes) + are prototypes so that the instancing can be done efficiently. + + In such cases, it is assumed that the prototypes have sim-ready properties on them that don't need to be modified. + Trying to set properties into prototypes will throw a warning saying that the prim is a prototype and the + properties cannot be set. + +The schemas are defined in the following links: + +* `UsdPhysics schema `_ +* `PhysxSchema schema `_ + +Locally, the schemas are defined in the following files: + +* ``_isaac_sim/extsPhysics/omni.usd.schema.physics/plugins/UsdPhysics/resources/UsdPhysics/schema.usda`` +* ``_isaac_sim/extsPhysics/omni.usd.schema.physx/plugins/PhysxSchema/resources/generatedSchema.usda`` + +""" + +from .schemas import ( + activate_contact_sensors, + define_articulation_root_properties, + define_collision_properties, + define_deformable_body_properties, + define_mass_properties, + define_rigid_body_properties, + modify_articulation_root_properties, + modify_collision_properties, + modify_deformable_body_properties, + modify_fixed_tendon_properties, + modify_joint_drive_properties, + modify_mass_properties, + modify_rigid_body_properties, +) +from .schemas_cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, + DeformableBodyPropertiesCfg, + FixedTendonPropertiesCfg, + JointDrivePropertiesCfg, + MassPropertiesCfg, + RigidBodyPropertiesCfg, +) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas.py new file mode 100644 index 00000000000..5699a3a41ca --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas.py @@ -0,0 +1,858 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# needed to import for allowing type-hinting: Usd.Stage | None +from __future__ import annotations + +import math + +import isaacsim.core.utils.stage as stage_utils +import omni.log +import omni.physx.scripts.utils as physx_utils +from omni.physx.scripts import deformableUtils as deformable_utils +from pxr import PhysxSchema, Usd, UsdPhysics + +from ..utils import ( + apply_nested, + find_global_fixed_joint_prim, + get_all_matching_child_prims, + safe_set_attribute_on_usd_schema, +) +from . import schemas_cfg + +""" +Articulation root properties. +""" + + +def define_articulation_root_properties( + prim_path: str, cfg: schemas_cfg.ArticulationRootPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the articulation root schema on the input prim and set its properties. + + See :func:`modify_articulation_root_properties` for more details on how the properties are set. + + Args: + prim_path: The prim path where to apply the articulation root schema. + cfg: The configuration for the articulation root. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: When the prim path is not valid. + TypeError: When the prim already has conflicting API schemas. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get articulation USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + # check if prim has articulation applied on it + if not UsdPhysics.ArticulationRootAPI(prim): + UsdPhysics.ArticulationRootAPI.Apply(prim) + # set articulation root properties + modify_articulation_root_properties(prim_path, cfg, stage) + + +@apply_nested +def modify_articulation_root_properties( + prim_path: str, cfg: schemas_cfg.ArticulationRootPropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX parameters for an articulation root prim. + + The `articulation root`_ marks the root of an articulation tree. For floating articulations, this should be on + the root body. For fixed articulations, this API can be on a direct or indirect parent of the root joint + which is fixed to the world. + + The schema comprises of attributes that belong to the `ArticulationRootAPI`_ and `PhysxArticulationAPI`_. + schemas. The latter contains the PhysX parameters for the articulation root. + + The properties are applied to the articulation root prim. The common properties (such as solver position + and velocity iteration counts, sleep threshold, stabilization threshold) take precedence over those specified + in the rigid body schemas for all the rigid bodies in the articulation. + + .. caution:: + When the attribute :attr:`schemas_cfg.ArticulationRootPropertiesCfg.fix_root_link` is set to True, + a fixed joint is created between the root link and the world frame (if it does not already exist). However, + to deal with physics parser limitations, the articulation root schema needs to be applied to the parent of + the root link. + + .. note:: + This function is decorated with :func:`apply_nested` that set the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _articulation root: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Articulations.html + .. _ArticulationRootAPI: https://openusd.org/dev/api/class_usd_physics_articulation_root_a_p_i.html + .. _PhysxArticulationAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_articulation_a_p_i.html + + Args: + prim_path: The prim path to the articulation root. + cfg: The configuration for the articulation root. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + + Raises: + NotImplementedError: When the root prim is not a rigid body and a fixed joint is to be created. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get articulation USD prim + articulation_prim = stage.GetPrimAtPath(prim_path) + # check if prim has articulation applied on it + if not UsdPhysics.ArticulationRootAPI(articulation_prim): + return False + # retrieve the articulation api + physx_articulation_api = PhysxSchema.PhysxArticulationAPI(articulation_prim) + if not physx_articulation_api: + physx_articulation_api = PhysxSchema.PhysxArticulationAPI.Apply(articulation_prim) + + # convert to dict + cfg = cfg.to_dict() + # extract non-USD properties + fix_root_link = cfg.pop("fix_root_link", None) + + # set into physx api + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_articulation_api, attr_name, value, camel_case=True) + + # fix root link based on input + # we do the fixed joint processing later to not interfere with setting other properties + if fix_root_link is not None: + # check if a global fixed joint exists under the root prim + existing_fixed_joint_prim = find_global_fixed_joint_prim(prim_path) + + # if we found a fixed joint, enable/disable it based on the input + # otherwise, create a fixed joint between the world and the root link + if existing_fixed_joint_prim is not None: + omni.log.info( + f"Found an existing fixed joint for the articulation: '{prim_path}'. Setting it to: {fix_root_link}." + ) + existing_fixed_joint_prim.GetJointEnabledAttr().Set(fix_root_link) + elif fix_root_link: + omni.log.info(f"Creating a fixed joint for the articulation: '{prim_path}'.") + + # note: we have to assume that the root prim is a rigid body, + # i.e. we don't handle the case where the root prim is not a rigid body but has articulation api on it + # Currently, there is no obvious way to get first rigid body link identified by the PhysX parser + if not articulation_prim.HasAPI(UsdPhysics.RigidBodyAPI): + raise NotImplementedError( + f"The articulation prim '{prim_path}' does not have the RigidBodyAPI applied." + " To create a fixed joint, we need to determine the first rigid body link in" + " the articulation tree. However, this is not implemented yet." + ) + + # create a fixed joint between the root link and the world frame + physx_utils.createJoint(stage=stage, joint_type="Fixed", from_prim=None, to_prim=articulation_prim) + + # Having a fixed joint on a rigid body is not treated as "fixed base articulation". + # instead, it is treated as a part of the maximal coordinate tree. + # Moving the articulation root to the parent solves this issue. This is a limitation of the PhysX parser. + # get parent prim + parent_prim = articulation_prim.GetParent() + # apply api to parent + UsdPhysics.ArticulationRootAPI.Apply(parent_prim) + PhysxSchema.PhysxArticulationAPI.Apply(parent_prim) + + # copy the attributes + # -- usd attributes + usd_articulation_api = UsdPhysics.ArticulationRootAPI(articulation_prim) + for attr_name in usd_articulation_api.GetSchemaAttributeNames(): + attr = articulation_prim.GetAttribute(attr_name) + parent_prim.GetAttribute(attr_name).Set(attr.Get()) + # -- physx attributes + physx_articulation_api = PhysxSchema.PhysxArticulationAPI(articulation_prim) + for attr_name in physx_articulation_api.GetSchemaAttributeNames(): + attr = articulation_prim.GetAttribute(attr_name) + parent_prim.GetAttribute(attr_name).Set(attr.Get()) + + # remove api from root + articulation_prim.RemoveAPI(UsdPhysics.ArticulationRootAPI) + articulation_prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI) + + # success + return True + + +""" +Rigid body properties. +""" + + +def define_rigid_body_properties( + prim_path: str, cfg: schemas_cfg.RigidBodyPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the rigid body schema on the input prim and set its properties. + + See :func:`modify_rigid_body_properties` for more details on how the properties are set. + + Args: + prim_path: The prim path where to apply the rigid body schema. + cfg: The configuration for the rigid body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: When the prim path is not valid. + TypeError: When the prim already has conflicting API schemas. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + # check if prim has rigid body applied on it + if not UsdPhysics.RigidBodyAPI(prim): + UsdPhysics.RigidBodyAPI.Apply(prim) + # set rigid body properties + modify_rigid_body_properties(prim_path, cfg, stage) + + +@apply_nested +def modify_rigid_body_properties( + prim_path: str, cfg: schemas_cfg.RigidBodyPropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX parameters for a rigid body prim. + + A `rigid body`_ is a single body that can be simulated by PhysX. It can be either dynamic or kinematic. + A dynamic body responds to forces and collisions. A `kinematic body`_ can be moved by the user, but does not + respond to forces. They are similar to having static bodies that can be moved around. + + The schema comprises of attributes that belong to the `RigidBodyAPI`_ and `PhysxRigidBodyAPI`_. + schemas. The latter contains the PhysX parameters for the rigid body. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _rigid body: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/RigidBodyOverview.html + .. _kinematic body: https://openusd.org/release/wp_rigid_body_physics.html#kinematic-bodies + .. _RigidBodyAPI: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html + .. _PhysxRigidBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_rigid_body_a_p_i.html + + Args: + prim_path: The prim path to the rigid body. + cfg: The configuration for the rigid body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get rigid-body USD prim + rigid_body_prim = stage.GetPrimAtPath(prim_path) + # check if prim has rigid-body applied on it + if not UsdPhysics.RigidBodyAPI(rigid_body_prim): + return False + # retrieve the USD rigid-body api + usd_rigid_body_api = UsdPhysics.RigidBodyAPI(rigid_body_prim) + # retrieve the physx rigid-body api + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim) + if not physx_rigid_body_api: + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim) + + # convert to dict + cfg = cfg.to_dict() + # set into USD API + for attr_name in ["rigid_body_enabled", "kinematic_enabled"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_rigid_body_api, attr_name, value, camel_case=True) + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_rigid_body_api, attr_name, value, camel_case=True) + # success + return True + + +""" +Collision properties. +""" + + +def define_collision_properties( + prim_path: str, cfg: schemas_cfg.CollisionPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the collision schema on the input prim and set its properties. + + See :func:`modify_collision_properties` for more details on how the properties are set. + + Args: + prim_path: The prim path where to apply the rigid body schema. + cfg: The configuration for the collider. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: When the prim path is not valid. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + # check if prim has collision applied on it + if not UsdPhysics.CollisionAPI(prim): + UsdPhysics.CollisionAPI.Apply(prim) + # set collision properties + modify_collision_properties(prim_path, cfg, stage) + + +@apply_nested +def modify_collision_properties( + prim_path: str, cfg: schemas_cfg.CollisionPropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX properties of collider prim. + + These properties are based on the `UsdPhysics.CollisionAPI`_ and `PhysxSchema.PhysxCollisionAPI`_ schemas. + For more information on the properties, please refer to the official documentation. + + Tuning these parameters influence the contact behavior of the rigid body. For more information on + tune them and their effect on the simulation, please refer to the + `PhysX documentation `__. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _UsdPhysics.CollisionAPI: https://openusd.org/dev/api/class_usd_physics_collision_a_p_i.html + .. _PhysxSchema.PhysxCollisionAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_collision_a_p_i.html + + Args: + prim_path: The prim path of parent. + cfg: The configuration for the collider. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + collider_prim = stage.GetPrimAtPath(prim_path) + # check if prim has collision applied on it + if not UsdPhysics.CollisionAPI(collider_prim): + return False + # retrieve the USD collision api + usd_collision_api = UsdPhysics.CollisionAPI(collider_prim) + # retrieve the collision api + physx_collision_api = PhysxSchema.PhysxCollisionAPI(collider_prim) + if not physx_collision_api: + physx_collision_api = PhysxSchema.PhysxCollisionAPI.Apply(collider_prim) + + # convert to dict + cfg = cfg.to_dict() + # set into USD API + for attr_name in ["collision_enabled"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_collision_api, attr_name, value, camel_case=True) + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_collision_api, attr_name, value, camel_case=True) + # success + return True + + +""" +Mass properties. +""" + + +def define_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, stage: Usd.Stage | None = None): + """Apply the mass schema on the input prim and set its properties. + + See :func:`modify_mass_properties` for more details on how the properties are set. + + Args: + prim_path: The prim path where to apply the rigid body schema. + cfg: The configuration for the mass properties. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: When the prim path is not valid. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + # check if prim has mass applied on it + if not UsdPhysics.MassAPI(prim): + UsdPhysics.MassAPI.Apply(prim) + # set mass properties + modify_mass_properties(prim_path, cfg, stage) + + +@apply_nested +def modify_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, stage: Usd.Stage | None = None) -> bool: + """Set properties for the mass of a rigid body prim. + + These properties are based on the `UsdPhysics.MassAPI` schema. If the mass is not defined, the density is used + to compute the mass. However, in that case, a collision approximation of the rigid body is used to + compute the density. For more information on the properties, please refer to the + `documentation `__. + + .. caution:: + + The mass of an object can be specified in multiple ways and have several conflicting settings + that are resolved based on precedence. Please make sure to understand the precedence rules + before using this property. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. UsdPhysics.MassAPI: https://openusd.org/dev/api/class_usd_physics_mass_a_p_i.html + + Args: + prim_path: The prim path of the rigid body. + cfg: The configuration for the mass properties. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + rigid_prim = stage.GetPrimAtPath(prim_path) + # check if prim has mass API applied on it + if not UsdPhysics.MassAPI(rigid_prim): + return False + # retrieve the USD mass api + usd_physics_mass_api = UsdPhysics.MassAPI(rigid_prim) + + # convert to dict + cfg = cfg.to_dict() + # set into USD API + for attr_name in ["mass", "density"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_physics_mass_api, attr_name, value, camel_case=True) + # success + return True + + +""" +Contact sensor. +""" + + +def activate_contact_sensors(prim_path: str, threshold: float = 0.0, stage: Usd.Stage = None): + """Activate the contact sensor on all rigid bodies under a specified prim path. + + This function adds the PhysX contact report API to all rigid bodies under the specified prim path. + It also sets the force threshold beyond which the contact sensor reports the contact. The contact + reporting API can only be added to rigid bodies. + + Args: + prim_path: The prim path under which to search and prepare contact sensors. + threshold: The threshold for the contact sensor. Defaults to 0.0. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: If the input prim path is not valid. + ValueError: If there are no rigid bodies under the prim path. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get prim + prim: Usd.Prim = stage.GetPrimAtPath(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + # iterate over all children + num_contact_sensors = 0 + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # check if prim is a rigid body + # nested rigid bodies are not allowed by SDK so we can safely assume that + # if a prim has a rigid body API, it is a rigid body and we don't need to + # check its children + if child_prim.HasAPI(UsdPhysics.RigidBodyAPI): + # set sleep threshold to zero + rb = PhysxSchema.PhysxRigidBodyAPI.Get(stage, prim.GetPrimPath()) + rb.CreateSleepThresholdAttr().Set(0.0) + # add contact report API with threshold of zero + if not child_prim.HasAPI(PhysxSchema.PhysxContactReportAPI): + omni.log.verbose(f"Adding contact report API to prim: '{child_prim.GetPrimPath()}'") + cr_api = PhysxSchema.PhysxContactReportAPI.Apply(child_prim) + else: + omni.log.verbose(f"Contact report API already exists on prim: '{child_prim.GetPrimPath()}'") + cr_api = PhysxSchema.PhysxContactReportAPI.Get(stage, child_prim.GetPrimPath()) + # set threshold to zero + cr_api.CreateThresholdAttr().Set(threshold) + # increment number of contact sensors + num_contact_sensors += 1 + else: + # add all children to tree + all_prims += child_prim.GetChildren() + # check if no contact sensors were found + if num_contact_sensors == 0: + raise ValueError( + f"No contact sensors added to the prim: '{prim_path}'. This means that no rigid bodies" + " are present under this prim. Please check the prim path." + ) + # success + return True + + +""" +Joint drive properties. +""" + + +@apply_nested +def modify_joint_drive_properties( + prim_path: str, cfg: schemas_cfg.JointDrivePropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX parameters for a joint prim. + + This function checks if the input prim is a prismatic or revolute joint and applies the joint drive schema + on it. If the joint is a tendon (i.e., it has the `PhysxTendonAxisAPI`_ schema applied on it), then the joint + drive schema is not applied. + + Based on the configuration, this method modifies the properties of the joint drive. These properties are + based on the `UsdPhysics.DriveAPI`_ schema. For more information on the properties, please refer to the + official documentation. + + .. caution:: + + We highly recommend modifying joint properties of articulations through the functionalities in the + :mod:`isaaclab.actuators` module. The methods here are for setting simulation low-level + properties only. + + .. _UsdPhysics.DriveAPI: https://openusd.org/dev/api/class_usd_physics_drive_a_p_i.html + .. _PhysxTendonAxisAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_axis_a_p_i.html + + Args: + prim_path: The prim path where to apply the joint drive schema. + cfg: The configuration for the joint drive. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + + Raises: + ValueError: If the input prim path is not valid. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + + # check if prim has joint drive applied on it + if prim.IsA(UsdPhysics.RevoluteJoint): + drive_api_name = "angular" + elif prim.IsA(UsdPhysics.PrismaticJoint): + drive_api_name = "linear" + else: + return False + # check that prim is not a tendon child prim + # note: root prim is what "controls" the tendon so we still want to apply the drive to it + if prim.HasAPI(PhysxSchema.PhysxTendonAxisAPI) and not prim.HasAPI(PhysxSchema.PhysxTendonAxisRootAPI): + return False + + # check if prim has joint drive applied on it + usd_drive_api = UsdPhysics.DriveAPI(prim, drive_api_name) + if not usd_drive_api: + usd_drive_api = UsdPhysics.DriveAPI.Apply(prim, drive_api_name) + # check if prim has Physx joint drive applied on it + physx_joint_api = PhysxSchema.PhysxJointAPI(prim) + if not physx_joint_api: + physx_joint_api = PhysxSchema.PhysxJointAPI.Apply(prim) + + # mapping from configuration name to USD attribute name + cfg_to_usd_map = { + "max_velocity": "max_joint_velocity", + "max_effort": "max_force", + "drive_type": "type", + } + # convert to dict + cfg = cfg.to_dict() + + # check if linear drive + is_linear_drive = prim.IsA(UsdPhysics.PrismaticJoint) + # convert values for angular drives from radians to degrees units + if not is_linear_drive: + if cfg["max_velocity"] is not None: + # rad / s --> deg / s + cfg["max_velocity"] = cfg["max_velocity"] * 180.0 / math.pi + if cfg["stiffness"] is not None: + # N-m/rad --> N-m/deg + cfg["stiffness"] = cfg["stiffness"] * math.pi / 180.0 + if cfg["damping"] is not None: + # N-m-s/rad --> N-m-s/deg + cfg["damping"] = cfg["damping"] * math.pi / 180.0 + + # set into PhysX API + for attr_name in ["max_velocity"]: + value = cfg.pop(attr_name, None) + attr_name = cfg_to_usd_map[attr_name] + safe_set_attribute_on_usd_schema(physx_joint_api, attr_name, value, camel_case=True) + # set into USD API + for attr_name, attr_value in cfg.items(): + attr_name = cfg_to_usd_map.get(attr_name, attr_name) + safe_set_attribute_on_usd_schema(usd_drive_api, attr_name, attr_value, camel_case=True) + + return True + + +""" +Fixed tendon properties. +""" + + +@apply_nested +def modify_fixed_tendon_properties( + prim_path: str, cfg: schemas_cfg.FixedTendonPropertiesCfg, stage: Usd.Stage | None = None +) -> bool: + """Modify PhysX parameters for a fixed tendon attachment prim. + + A `fixed tendon`_ can be used to link multiple degrees of freedom of articulation joints + through length and limit constraints. For instance, it can be used to set up an equality constraint + between a driven and passive revolute joints. + + The schema comprises of attributes that belong to the `PhysxTendonAxisRootAPI`_ schema. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _fixed tendon: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxArticulationFixedTendon.html + .. _PhysxTendonAxisRootAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_axis_root_a_p_i.html + + Args: + prim_path: The prim path to the tendon attachment. + cfg: The configuration for the tendon attachment. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + + Raises: + ValueError: If the input prim path is not valid. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + tendon_prim = stage.GetPrimAtPath(prim_path) + # check if prim has fixed tendon applied on it + has_root_fixed_tendon = tendon_prim.HasAPI(PhysxSchema.PhysxTendonAxisRootAPI) + if not has_root_fixed_tendon: + return False + + # resolve all available instances of the schema since it is multi-instance + for schema_name in tendon_prim.GetAppliedSchemas(): + # only consider the fixed tendon schema + if "PhysxTendonAxisRootAPI" not in schema_name: + continue + # retrieve the USD tendon api + instance_name = schema_name.split(":")[-1] + physx_tendon_axis_api = PhysxSchema.PhysxTendonAxisRootAPI(tendon_prim, instance_name) + + # convert to dict + cfg = cfg.to_dict() + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_tendon_axis_api, attr_name, value, camel_case=True) + # success + return True + + +""" +Deformable body properties. +""" + + +def define_deformable_body_properties( + prim_path: str, cfg: schemas_cfg.DeformableBodyPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the deformable body schema on the input prim and set its properties. + + See :func:`modify_deformable_body_properties` for more details on how the properties are set. + + .. note:: + If the input prim is not a mesh, this function will traverse the prim and find the first mesh + under it. If no mesh or multiple meshes are found, an error is raised. This is because the deformable + body schema can only be applied to a single mesh. + + Args: + prim_path: The prim path where to apply the deformable body schema. + cfg: The configuration for the deformable body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: When the prim path is not valid. + ValueError: When the prim has no mesh or multiple meshes. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + + # traverse the prim and get the mesh + matching_prims = get_all_matching_child_prims(prim_path, lambda p: p.GetTypeName() == "Mesh") + # check if the mesh is valid + if len(matching_prims) == 0: + raise ValueError(f"Could not find any mesh in '{prim_path}'. Please check asset.") + if len(matching_prims) > 1: + # get list of all meshes found + mesh_paths = [p.GetPrimPath() for p in matching_prims] + raise ValueError( + f"Found multiple meshes in '{prim_path}': {mesh_paths}." + " Deformable body schema can only be applied to one mesh." + ) + + # get deformable-body USD prim + mesh_prim = matching_prims[0] + # check if prim has deformable-body applied on it + if not PhysxSchema.PhysxDeformableBodyAPI(mesh_prim): + PhysxSchema.PhysxDeformableBodyAPI.Apply(mesh_prim) + # set deformable body properties + modify_deformable_body_properties(mesh_prim.GetPrimPath(), cfg, stage) + + +@apply_nested +def modify_deformable_body_properties( + prim_path: str, cfg: schemas_cfg.DeformableBodyPropertiesCfg, stage: Usd.Stage | None = None +): + """Modify PhysX parameters for a deformable body prim. + + A `deformable body`_ is a single body that can be simulated by PhysX. Unlike rigid bodies, deformable bodies + support relative motion of the nodes in the mesh. Consequently, they can be used to simulate deformations + under applied forces. + + PhysX soft body simulation employs Finite Element Analysis (FEA) to simulate the deformations of the mesh. + It uses two tetrahedral meshes to represent the deformable body: + + 1. **Simulation mesh**: This mesh is used for the simulation and is the one that is deformed by the solver. + 2. **Collision mesh**: This mesh only needs to match the surface of the simulation mesh and is used for + collision detection. + + For most applications, we assume that the above two meshes are computed from the "render mesh" of the deformable + body. The render mesh is the mesh that is visible in the scene and is used for rendering purposes. It is composed + of triangles and is the one that is used to compute the above meshes based on PhysX cookings. + + The schema comprises of attributes that belong to the `PhysxDeformableBodyAPI`_. schemas containing the PhysX + parameters for the deformable body. + + .. caution:: + The deformable body schema is still under development by the Omniverse team. The current implementation + works with the PhysX schemas shipped with Isaac Sim 4.0.0 onwards. It may change in future releases. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _deformable body: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/SoftBodies.html + .. _PhysxDeformableBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_deformable_a_p_i.html + + Args: + prim_path: The prim path to the deformable body. + cfg: The configuration for the deformable body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + + # get deformable-body USD prim + deformable_body_prim = stage.GetPrimAtPath(prim_path) + + # check if the prim is valid and has the deformable-body API + if not deformable_body_prim.IsValid() or not PhysxSchema.PhysxDeformableBodyAPI(deformable_body_prim): + return False + + # retrieve the physx deformable-body api + physx_deformable_body_api = PhysxSchema.PhysxDeformableBodyAPI(deformable_body_prim) + # retrieve the physx deformable api + physx_deformable_api = PhysxSchema.PhysxDeformableAPI(physx_deformable_body_api) + + # convert to dict + cfg = cfg.to_dict() + # set into deformable body API + attr_kwargs = { + attr_name: cfg.pop(attr_name) + for attr_name in [ + "kinematic_enabled", + "collision_simplification", + "collision_simplification_remeshing", + "collision_simplification_remeshing_resolution", + "collision_simplification_target_triangle_count", + "collision_simplification_force_conforming", + "simulation_hexahedral_resolution", + "solver_position_iteration_count", + "vertex_velocity_damping", + "sleep_damping", + "sleep_threshold", + "settling_threshold", + "self_collision", + "self_collision_filter_distance", + ] + } + status = deformable_utils.add_physx_deformable_body(stage, prim_path=prim_path, **attr_kwargs) + # check if the deformable body was successfully added + if not status: + return False + + # obtain the PhysX collision API (this is set when the deformable body is added) + physx_collision_api = PhysxSchema.PhysxCollisionAPI(deformable_body_prim) + + # set into PhysX API + for attr_name, value in cfg.items(): + if attr_name in ["rest_offset", "contact_offset"]: + safe_set_attribute_on_usd_schema(physx_collision_api, attr_name, value, camel_case=True) + else: + safe_set_attribute_on_usd_schema(physx_deformable_api, attr_name, value, camel_case=True) + + # success + return True diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas_cfg.py new file mode 100644 index 00000000000..56b496286a8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/schemas/schemas_cfg.py @@ -0,0 +1,402 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from typing import Literal + +from isaaclab.utils import configclass + + +@configclass +class ArticulationRootPropertiesCfg: + """Properties to apply to the root of an articulation. + + See :meth:`modify_articulation_root_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + articulation_enabled: bool | None = None + """Whether to enable or disable articulation.""" + + enabled_self_collisions: bool | None = None + """Whether to enable or disable self-collisions.""" + + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + + solver_velocity_iteration_count: int | None = None + """Solver velocity iteration counts for the body.""" + + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an articulation may participate in stabilization.""" + + fix_root_link: bool | None = None + """Whether to fix the root link of the articulation. + + * If set to None, the root link is not modified. + * If the articulation already has a fixed root link, this flag will enable or disable the fixed joint. + * If the articulation does not have a fixed root link, this flag will create a fixed joint between the world + frame and the root link. The joint is created with the name "FixedJoint" under the articulation prim. + + .. note:: + This is a non-USD schema property. It is handled by the :meth:`modify_articulation_root_properties` function. + + """ + + +@configclass +class RigidBodyPropertiesCfg: + """Properties to apply to a rigid body. + + See :meth:`modify_rigid_body_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + rigid_body_enabled: bool | None = None + """Whether to enable or disable the rigid body.""" + + kinematic_enabled: bool | None = None + """Determines whether the body is kinematic or not. + + A kinematic body is a body that is moved through animated poses or through user defined poses. The simulation + still derives velocities for the kinematic body based on the external motion. + + For more information on kinematic bodies, please refer to the `documentation `_. + """ + + disable_gravity: bool | None = None + """Disable gravity for the actor.""" + + linear_damping: float | None = None + """Linear damping for the body.""" + + angular_damping: float | None = None + """Angular damping for the body.""" + + max_linear_velocity: float | None = None + """Maximum linear velocity for rigid bodies (in m/s).""" + + max_angular_velocity: float | None = None + """Maximum angular velocity for rigid bodies (in deg/s).""" + + max_depenetration_velocity: float | None = None + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" + + max_contact_impulse: float | None = None + """The limit on the impulse that may be applied at a contact.""" + + enable_gyroscopic_forces: bool | None = None + """Enables computation of gyroscopic forces on the rigid body.""" + + retain_accelerations: bool | None = None + """Carries over forces/accelerations over sub-steps.""" + + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + + solver_velocity_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an actor may participate in stabilization.""" + + +@configclass +class CollisionPropertiesCfg: + """Properties to apply to colliders in a rigid body. + + See :meth:`modify_collision_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + collision_enabled: bool | None = None + """Whether to enable or disable collisions.""" + + contact_offset: float | None = None + """Contact offset for the collision shape (in m). + + The collision detector generates contact points as soon as two shapes get closer than the sum of their + contact offsets. This quantity should be non-negative which means that contact generation can potentially start + before the shapes actually penetrate. + """ + + rest_offset: float | None = None + """Rest offset for the collision shape (in m). + + The rest offset quantifies how close a shape gets to others at rest, At rest, the distance between two + vertically stacked objects is the sum of their rest offsets. If a pair of shapes have a positive rest + offset, the shapes will be separated at rest by an air gap. + """ + + torsional_patch_radius: float | None = None + """Radius of the contact patch for applying torsional friction (in m). + + It is used to approximate rotational friction introduced by the compression of contacting surfaces. + If the radius is zero, no torsional friction is applied. + """ + + min_torsional_patch_radius: float | None = None + """Minimum radius of the contact patch for applying torsional friction (in m).""" + + +@configclass +class MassPropertiesCfg: + """Properties to define explicit mass properties of a rigid body. + + See :meth:`modify_mass_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + mass: float | None = None + """The mass of the rigid body (in kg). + + Note: + If non-zero, the mass is ignored and the density is used to compute the mass. + """ + + density: float | None = None + """The density of the rigid body (in kg/m^3). + + The density indirectly defines the mass of the rigid body. It is generally computed using the collision + approximation of the body. + """ + + +@configclass +class JointDrivePropertiesCfg: + """Properties to define the drive mechanism of a joint. + + See :meth:`modify_joint_drive_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + drive_type: Literal["force", "acceleration"] | None = None + """Joint drive type to apply. + + If the drive type is "force", then the joint is driven by a force. If the drive type is "acceleration", + then the joint is driven by an acceleration (usually used for kinematic joints). + """ + + max_effort: float | None = None + """Maximum effort that can be applied to the joint (in kg-m^2/s^2).""" + + max_velocity: float | None = None + """Maximum velocity of the joint. + + The unit depends on the joint model: + + * For linear joints, the unit is m/s. + * For angular joints, the unit is rad/s. + """ + + stiffness: float | None = None + """Stiffness of the joint drive. + + The unit depends on the joint model: + + * For linear joints, the unit is kg-m/s^2 (N/m). + * For angular joints, the unit is kg-m^2/s^2/rad (N-m/rad). + """ + + damping: float | None = None + """Damping of the joint drive. + + The unit depends on the joint model: + + * For linear joints, the unit is kg-m/s (N-s/m). + * For angular joints, the unit is kg-m^2/s/rad (N-m-s/rad). + """ + + +@configclass +class FixedTendonPropertiesCfg: + """Properties to define fixed tendons of an articulation. + + See :meth:`modify_fixed_tendon_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + tendon_enabled: bool | None = None + """Whether to enable or disable the tendon.""" + + stiffness: float | None = None + """Spring stiffness term acting on the tendon's length.""" + + damping: float | None = None + """The damping term acting on both the tendon length and the tendon-length limits.""" + + limit_stiffness: float | None = None + """Limit stiffness term acting on the tendon's length limits.""" + + offset: float | None = None + """Length offset term for the tendon. + + It defines an amount to be added to the accumulated length computed for the tendon. This allows the application + to actuate the tendon by shortening or lengthening it. + """ + + rest_length: float | None = None + """Spring rest length of the tendon.""" + + +@configclass +class DeformableBodyPropertiesCfg: + """Properties to apply to a deformable body. + + A deformable body is a body that can deform under forces. The configuration allows users to specify + the properties of the deformable body, such as the solver iteration counts, damping, and self-collision. + + An FEM-based deformable body is created by providing a collision mesh and simulation mesh. The collision mesh + is used for collision detection and the simulation mesh is used for simulation. The collision mesh is usually + a simplified version of the simulation mesh. + + Based on the above, the PhysX team provides APIs to either set the simulation and collision mesh directly + (by specifying the points) or to simplify the collision mesh based on the simulation mesh. The simplification + process involves remeshing the collision mesh and simplifying it based on the target triangle count. + + Since specifying the collision mesh points directly is not a common use case, we only expose the parameters + to simplify the collision mesh based on the simulation mesh. If you want to provide the collision mesh points, + please open an issue on the repository and we can add support for it. + + See :meth:`modify_deformable_body_properties` for more information. + + .. note:: + If the values are :obj:`None`, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + deformable_enabled: bool | None = None + """Enables deformable body.""" + + kinematic_enabled: bool = False + """Enables kinematic body. Defaults to False, which means that the body is not kinematic. + + Similar to rigid bodies, this allows setting user-driven motion for the deformable body. For more information, + please refer to the `documentation `__. + """ + + self_collision: bool | None = None + """Whether to enable or disable self-collisions for the deformable body based on the rest position distances.""" + + self_collision_filter_distance: float | None = None + """Penetration value that needs to get exceeded before contacts for self collision are generated. + + This parameter must be greater than of equal to twice the :attr:`rest_offset` value. + + This value has an effect only if :attr:`self_collision` is enabled. + """ + + settling_threshold: float | None = None + """Threshold vertex velocity (in m/s) under which sleep damping is applied in addition to velocity damping.""" + + sleep_damping: float | None = None + """Coefficient for the additional damping term if fertex velocity drops below setting threshold.""" + + sleep_threshold: float | None = None + """The velocity threshold (in m/s) under which the vertex becomes a candidate for sleeping in the next step.""" + + solver_position_iteration_count: int | None = None + """Number of the solver positional iterations per step. Range is [1,255]""" + + vertex_velocity_damping: float | None = None + """Coefficient for artificial damping on the vertex velocity. + + This parameter can be used to approximate the effect of air drag on the deformable body. + """ + + simulation_hexahedral_resolution: int = 10 + """The target resolution for the hexahedral mesh used for simulation. Defaults to 10. + + Note: + This value is ignored if the user provides the simulation mesh points directly. However, we assume that + most users will not provide the simulation mesh points directly. If you want to provide the simulation mesh + directly, please set this value to :obj:`None`. + """ + + collision_simplification: bool = True + """Whether or not to simplify the collision mesh before creating a soft body out of it. Defaults to True. + + Note: + This flag is ignored if the user provides the simulation mesh points directly. However, we assume that + most users will not provide the simulation mesh points directly. Hence, this flag is enabled by default. + + If you want to provide the simulation mesh points directly, please set this flag to False. + """ + + collision_simplification_remeshing: bool = True + """Whether or not the collision mesh should be remeshed before simplification. Defaults to True. + + This parameter is ignored if :attr:`collision_simplification` is False. + """ + + collision_simplification_remeshing_resolution: int = 0 + """The resolution used for remeshing. Defaults to 0, which means that a heuristic is used to determine the + resolution. + + This parameter is ignored if :attr:`collision_simplification_remeshing` is False. + """ + + collision_simplification_target_triangle_count: int = 0 + """The target triangle count used for the simplification. Defaults to 0, which means that a heuristic based on + the :attr:`simulation_hexahedral_resolution` is used to determine the target count. + + This parameter is ignored if :attr:`collision_simplification` is False. + """ + + collision_simplification_force_conforming: bool = True + """Whether or not the simplification should force the output mesh to conform to the input mesh. Defaults to True. + + The flag indicates that the tretrahedralizer used to generate the collision mesh should produce tetrahedra + that conform to the triangle mesh. If False, the simplifier uses the output from the tretrahedralizer used. + + This parameter is ignored if :attr:`collision_simplification` is False. + """ + + contact_offset: float | None = None + """Contact offset for the collision shape (in m). + + The collision detector generates contact points as soon as two shapes get closer than the sum of their + contact offsets. This quantity should be non-negative which means that contact generation can potentially start + before the shapes actually penetrate. + """ + + rest_offset: float | None = None + """Rest offset for the collision shape (in m). + + The rest offset quantifies how close a shape gets to others at rest, At rest, the distance between two + vertically stacked objects is the sum of their rest offsets. If a pair of shapes have a positive rest + offset, the shapes will be separated at rest by an air gap. + """ + + max_depenetration_velocity: float | None = None + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_cfg.py new file mode 100644 index 00000000000..87fc01fcdba --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_cfg.py @@ -0,0 +1,332 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base configuration of the environment. + +This module defines the general configuration of the environment. It includes parameters for +configuring the environment instances, viewer settings, and simulation parameters. +""" + +from typing import Literal + +from isaaclab.utils import configclass + +from .spawners.materials import RigidBodyMaterialCfg + + +@configclass +class PhysxCfg: + """Configuration for PhysX solver-related parameters. + + These parameters are used to configure the PhysX solver. For more information, see the `PhysX 5 SDK + documentation`_. + + PhysX 5 supports GPU-accelerated physics simulation. This is enabled by default, but can be disabled + by setting the :attr:`~SimulationCfg.device` to ``cpu`` in :class:`SimulationCfg`. Unlike CPU PhysX, the GPU + simulation feature is unable to dynamically grow all the buffers. Therefore, it is necessary to provide + a reasonable estimate of the buffer sizes for GPU features. If insufficient buffer sizes are provided, the + simulation will fail with errors and lead to adverse behaviors. The buffer sizes can be adjusted through the + ``gpu_*`` parameters. + + .. _PhysX 5 SDK documentation: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxSceneDesc.html + + """ + + solver_type: Literal[0, 1] = 1 + """The type of solver to use.Default is 1 (TGS). + + Available solvers: + + * :obj:`0`: PGS (Projective Gauss-Seidel) + * :obj:`1`: TGS (Truncated Gauss-Seidel) + """ + + min_position_iteration_count: int = 1 + """Minimum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 1. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_position_iteration_count, max_position_iteration_count]``. + """ + + max_position_iteration_count: int = 255 + """Maximum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 255. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_position_iteration_count, max_position_iteration_count]``. + """ + + min_velocity_iteration_count: int = 0 + """Minimum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 0. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. + """ + + max_velocity_iteration_count: int = 255 + """Maximum number of solver velocity iterations (rigid bodies, cloth, particles etc.). Default is 255. + + .. note:: + + Each physics actor in Omniverse specifies its own solver iteration count. The solver takes + the number of iterations specified by the actor with the highest iteration and clamps it to + the range ``[min_velocity_iteration_count, max_velocity_iteration_count]``. + """ + + enable_ccd: bool = False + """Enable a second broad-phase pass that makes it possible to prevent objects from tunneling through each other. + Default is False.""" + + enable_stabilization: bool = True + """Enable/disable additional stabilization pass in solver. Default is True.""" + + enable_enhanced_determinism: bool = False + """Enable/disable improved determinism at the expense of performance. Defaults to False. + + For more information on PhysX determinism, please check `here`_. + + .. _here: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/RigidBodyDynamics.html#enhanced-determinism + """ + + bounce_threshold_velocity: float = 0.5 + """Relative velocity threshold for contacts to bounce (in m/s). Default is 0.5 m/s.""" + + friction_offset_threshold: float = 0.04 + """Threshold for contact point to experience friction force (in m). Default is 0.04 m.""" + + friction_correlation_distance: float = 0.025 + """Distance threshold for merging contacts into a single friction anchor point (in m). Default is 0.025 m.""" + + gpu_max_rigid_contact_count: int = 2**23 + """Size of rigid contact stream buffer allocated in pinned host memory. Default is 2 ** 23.""" + + gpu_max_rigid_patch_count: int = 5 * 2**15 + """Size of the rigid contact patch stream buffer allocated in pinned host memory. Default is 5 * 2 ** 15.""" + + gpu_found_lost_pairs_capacity: int = 2**21 + """Capacity of found and lost buffers allocated in GPU global memory. Default is 2 ** 21. + + This is used for the found/lost pair reports in the BP. + """ + + gpu_found_lost_aggregate_pairs_capacity: int = 2**25 + """Capacity of found and lost buffers in aggregate system allocated in GPU global memory. + Default is 2 ** 25. + + This is used for the found/lost pair reports in AABB manager. + """ + + gpu_total_aggregate_pairs_capacity: int = 2**21 + """Capacity of total number of aggregate pairs allocated in GPU global memory. Default is 2 ** 21.""" + + gpu_collision_stack_size: int = 2**26 + """Size of the collision stack buffer allocated in pinned host memory. Default is 2 ** 26.""" + + gpu_heap_capacity: int = 2**26 + """Initial capacity of the GPU and pinned host memory heaps. Additional memory will be allocated + if more memory is required. Default is 2 ** 26.""" + + gpu_temp_buffer_capacity: int = 2**24 + """Capacity of temp buffer allocated in pinned host memory. Default is 2 ** 24.""" + + gpu_max_num_partitions: int = 8 + """Limitation for the partitions in the GPU dynamics pipeline. Default is 8. + + This variable must be power of 2. A value greater than 32 is currently not supported. Range: (1, 32) + """ + + gpu_max_soft_body_contacts: int = 2**20 + """Size of soft body contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + gpu_max_particle_contacts: int = 2**20 + """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + + +@configclass +class RenderCfg: + """Configuration for Omniverse RTX Renderer. + + These parameters are used to configure the Omniverse RTX Renderer. The defaults for IsaacLab are set in the + experience files: `apps/isaaclab.python.rendering.kit` and `apps/isaaclab.python.headless.rendering.kit`. Setting any + value here will override the defaults of the experience files. + + For more information, see the `Omniverse RTX Renderer documentation`_. + + .. _Omniverse RTX Renderer documentation: https://docs.omniverse.nvidia.com/materials-and-rendering/latest/rtx-renderer.html + """ + + enable_translucency: bool | None = None + """Enables translucency for specular transmissive surfaces such as glass at the cost of some performance. Default is False. + + Set variable: /rtx/translucency/enabled + """ + + enable_reflections: bool | None = None + """Enables reflections at the cost of some performance. Default is False. + + Set variable: /rtx/reflections/enabled + """ + + enable_global_illumination: bool | None = None + """Enables Diffused Global Illumination at the cost of some performance. Default is False. + + Set variable: /rtx/indirectDiffuse/enabled + """ + + antialiasing_mode: Literal["Off", "FXAA", "DLSS", "TAA", "DLAA"] | None = None + """Selects the anti-aliasing mode to use. Defaults to DLSS. + - DLSS: Boosts performance by using AI to output higher resolution frames from a lower resolution input. DLSS samples multiple lower resolution images and uses motion data and feedback from prior frames to reconstruct native quality images. + - DLAA: Provides higher image quality with an AI-based anti-aliasing technique. DLAA uses the same Super Resolution technology developed for DLSS, reconstructing a native resolution image to maximize image quality. + + Set variable: /rtx/post/dlss/execMode + """ + + enable_dlssg: bool | None = None + """"Enables the use of DLSS-G. + DLSS Frame Generation boosts performance by using AI to generate more frames. + DLSS analyzes sequential frames and motion data to create additional high quality frames. + This feature requires an Ada Lovelace architecture GPU. + Enabling this feature also enables additional thread-related activities, which can hurt performance. + Default is False. + + Set variable: /rtx-transient/dlssg/enabled + """ + + enable_dl_denoiser: bool | None = None + """Enables the use of a DL denoiser. + The DL denoiser can help improve the quality of renders, but comes at a cost of performance. + + Set variable: /rtx-transient/dldenoiser/enabled + """ + + dlss_mode: Literal[0, 1, 2, 3] | None = None + """For DLSS anti-aliasing, selects the performance/quality tradeoff mode. + Valid values are 0 (Performance), 1 (Balanced), 2 (Quality), or 3 (Auto). Default is 0. + + Set variable: /rtx/post/dlss/execMode + """ + + enable_direct_lighting: bool | None = None + """Enable direct light contributions from lights. + + Set variable: /rtx/directLighting/enabled + """ + + samples_per_pixel: int | None = None + """Defines the Direct Lighting samples per pixel. + Higher values increase the direct lighting quality at the cost of performance. Default is 1. + + Set variable: /rtx/directLighting/sampledLighting/samplesPerPixel""" + + enable_shadows: bool | None = None + """Enables shadows at the cost of performance. When disabled, lights will not cast shadows. Defaults to True. + + Set variable: /rtx/shadows/enabled + """ + + enable_ambient_occlusion: bool | None = None + """Enables ambient occlusion at the cost of some performance. Default is False. + + Set variable: /rtx/ambientOcclusion/enabled + """ + + carb_settings: dict | None = None + """Provides a general dictionary for users to supply all carb rendering settings with native names. + - Name strings can be formatted like a carb setting, .kit file setting, or python variable. + - For instance, a key value pair can be + /rtx/translucency/enabled: False # carb + rtx.translucency.enabled: False # .kit + rtx_translucency_enabled: False # python""" + + rendering_mode: Literal["performance", "balanced", "quality", "xr"] | None = None + """Sets the rendering mode. Behaves the same as the CLI arg '--rendering_mode'""" + + +@configclass +class SimulationCfg: + """Configuration for simulation physics.""" + + physics_prim_path: str = "/physicsScene" + """The prim path where the USD PhysicsScene is created. Default is "/physicsScene".""" + + device: str = "cuda:0" + """The device to run the simulation on. Default is ``"cuda:0"``. + + Valid options are: + + - ``"cpu"``: Use CPU. + - ``"cuda"``: Use GPU, where the device ID is inferred from :class:`~isaaclab.app.AppLauncher`'s config. + - ``"cuda:N"``: Use GPU, where N is the device ID. For example, "cuda:0". + """ + + dt: float = 1.0 / 60.0 + """The physics simulation time-step (in seconds). Default is 0.0167 seconds.""" + + render_interval: int = 1 + """The number of physics simulation steps per rendering step. Default is 1.""" + + gravity: tuple[float, float, float] = (0.0, 0.0, -9.81) + """The gravity vector (in m/s^2). Default is (0.0, 0.0, -9.81). + + If set to (0.0, 0.0, 0.0), gravity is disabled. + """ + + enable_scene_query_support: bool = False + """Enable/disable scene query support for collision shapes. Default is False. + + This flag allows performing collision queries (raycasts, sweeps, and overlaps) on actors and + attached shapes in the scene. This is useful for implementing custom collision detection logic + outside of the physics engine. + + If set to False, the physics engine does not create the scene query manager and the scene query + functionality will not be available. However, this provides some performance speed-up. + + Note: + This flag is overridden to True inside the :class:`SimulationContext` class when running the simulation + with the GUI enabled. This is to allow certain GUI features to work properly. + """ + + use_fabric: bool = True + """Enable/disable reading of physics buffers directly. Default is True. + + When running the simulation, updates in the states in the scene is normally synchronized with USD. + This leads to an overhead in reading the data and does not scale well with massive parallelization. + This flag allows disabling the synchronization and reading the data directly from the physics buffers. + + It is recommended to set this flag to :obj:`True` when running the simulation with a large number + of primitives in the scene. + + Note: + When enabled, the GUI will not update the physics parameters in real-time. To enable real-time + updates, please set this flag to :obj:`False`. + """ + + physx: PhysxCfg = PhysxCfg() + """PhysX solver settings. Default is PhysxCfg().""" + + physics_material: RigidBodyMaterialCfg = RigidBodyMaterialCfg() + """Default physics material settings for rigid bodies. Default is RigidBodyMaterialCfg(). + + The physics engine defaults to this physics material for all the rigid body prims that do not have any + physics material specified on them. + + The material is created at the path: ``{physics_prim_path}/defaultMaterial``. + """ + + render: RenderCfg = RenderCfg() + """Render settings. Default is RenderCfg().""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_context.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_context.py new file mode 100644 index 00000000000..fa760f7ccc8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/simulation_context.py @@ -0,0 +1,871 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import builtins +import enum +import numpy as np +import os +import toml +import torch +import traceback +import weakref +from collections.abc import Iterator +from contextlib import contextmanager +from typing import Any + +import carb +import flatdict +import isaacsim.core.utils.stage as stage_utils +import omni.log +import omni.physx +from isaacsim.core.api.simulation_context import SimulationContext as _SimulationContext +from isaacsim.core.utils.carb import get_carb_setting, set_carb_setting +from isaacsim.core.utils.viewports import set_camera_view +from isaacsim.core.version import get_version +from pxr import Gf, PhysxSchema, Usd, UsdPhysics + +from .simulation_cfg import SimulationCfg +from .spawners import DomeLightCfg, GroundPlaneCfg +from .utils import bind_physics_material + + +class SimulationContext(_SimulationContext): + """A class to control simulation-related events such as physics stepping and rendering. + + The simulation context helps control various simulation aspects. This includes: + + * configure the simulator with different settings such as the physics time-step, the number of physics substeps, + and the physics solver parameters (for more information, see :class:`isaaclab.sim.SimulationCfg`) + * playing, pausing, stepping and stopping the simulation + * adding and removing callbacks to different simulation events such as physics stepping, rendering, etc. + + This class inherits from the :class:`isaacsim.core.api.simulation_context.SimulationContext` class and + adds additional functionalities such as setting up the simulation context with a configuration object, + exposing other commonly used simulator-related functions, and performing version checks of Isaac Sim + to ensure compatibility between releases. + + The simulation context is a singleton object. This means that there can only be one instance + of the simulation context at any given time. This is enforced by the parent class. Therefore, it is + not possible to create multiple instances of the simulation context. Instead, the simulation context + can be accessed using the ``instance()`` method. + + .. attention:: + Since we only support the `PyTorch `_ backend for simulation, the + simulation context is configured to use the ``torch`` backend by default. This means that + all the data structures used in the simulation are ``torch.Tensor`` objects. + + The simulation context can be used in two different modes of operations: + + 1. **Standalone python script**: In this mode, the user has full control over the simulation and + can trigger stepping events synchronously (i.e. as a blocking call). In this case the user + has to manually call :meth:`step` step the physics simulation and :meth:`render` to + render the scene. + 2. **Omniverse extension**: In this mode, the user has limited control over the simulation stepping + and all the simulation events are triggered asynchronously (i.e. as a non-blocking call). In this + case, the user can only trigger the simulation to start, pause, and stop. The simulation takes + care of stepping the physics simulation and rendering the scene. + + Based on above, for most functions in this class there is an equivalent function that is suffixed + with ``_async``. The ``_async`` functions are used in the Omniverse extension mode and + the non-``_async`` functions are used in the standalone python script mode. + """ + + class RenderMode(enum.IntEnum): + """Different rendering modes for the simulation. + + Render modes correspond to how the viewport and other UI elements (such as listeners to keyboard or mouse + events) are updated. There are three main components that can be updated when the simulation is rendered: + + 1. **UI elements and other extensions**: These are UI elements (such as buttons, sliders, etc.) and other + extensions that are running in the background that need to be updated when the simulation is running. + 2. **Cameras**: These are typically based on Hydra textures and are used to render the scene from different + viewpoints. They can be attached to a viewport or be used independently to render the scene. + 3. **Viewports**: These are windows where you can see the rendered scene. + + Updating each of the above components has a different overhead. For example, updating the viewports is + computationally expensive compared to updating the UI elements. Therefore, it is useful to be able to + control what is updated when the simulation is rendered. This is where the render mode comes in. There are + four different render modes: + + * :attr:`NO_GUI_OR_RENDERING`: The simulation is running without a GUI and off-screen rendering flag is disabled, + so none of the above are updated. + * :attr:`NO_RENDERING`: No rendering, where only 1 is updated at a lower rate. + * :attr:`PARTIAL_RENDERING`: Partial rendering, where only 1 and 2 are updated. + * :attr:`FULL_RENDERING`: Full rendering, where everything (1, 2, 3) is updated. + + .. _Viewports: https://docs.omniverse.nvidia.com/extensions/latest/ext_viewport.html + """ + + NO_GUI_OR_RENDERING = -1 + """The simulation is running without a GUI and off-screen rendering is disabled.""" + NO_RENDERING = 0 + """No rendering, where only other UI elements are updated at a lower rate.""" + PARTIAL_RENDERING = 1 + """Partial rendering, where the simulation cameras and UI elements are updated.""" + FULL_RENDERING = 2 + """Full rendering, where all the simulation viewports, cameras and UI elements are updated.""" + + def __init__(self, cfg: SimulationCfg | None = None): + """Creates a simulation context to control the simulator. + + Args: + cfg: The configuration of the simulation. Defaults to None, + in which case the default configuration is used. + """ + # store input + if cfg is None: + cfg = SimulationCfg() + # check that the config is valid + cfg.validate() + self.cfg = cfg + # check that simulation is running + if stage_utils.get_current_stage() is None: + raise RuntimeError("The stage has not been created. Did you run the simulator?") + + # acquire settings interface + self.carb_settings = carb.settings.get_settings() + + # apply carb physics settings + self._apply_physics_settings() + + # note: we read this once since it is not expected to change during runtime + # read flag for whether a local GUI is enabled + self._local_gui = self.carb_settings.get("/app/window/enabled") + # read flag for whether livestreaming GUI is enabled + self._livestream_gui = self.carb_settings.get("/app/livestream/enabled") + # read flag for whether XR GUI is enabled + self._xr_gui = self.carb_settings.get("/app/xr/enabled") + + # read flag for whether the Isaac Lab viewport capture pipeline will be used, + # casting None to False if the flag doesn't exist + # this flag is set from the AppLauncher class + self._offscreen_render = bool(self.carb_settings.get("/isaaclab/render/offscreen")) + # read flag for whether the default viewport should be enabled + self._render_viewport = bool(self.carb_settings.get("/isaaclab/render/active_viewport")) + # flag for whether any GUI will be rendered (local, livestreamed or viewport) + self._has_gui = self._local_gui or self._livestream_gui or self._xr_gui + + # apply render settings from render config + self._apply_render_settings_from_cfg() + + # store the default render mode + if not self._has_gui and not self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = self.RenderMode.NO_GUI_OR_RENDERING + # set viewport context to None + self._viewport_context = None + self._viewport_window = None + elif not self._has_gui and self._offscreen_render: + # set default render mode + # note: this is the terminal state: cannot exit from this render mode + self.render_mode = self.RenderMode.PARTIAL_RENDERING + # set viewport context to None + self._viewport_context = None + self._viewport_window = None + else: + # note: need to import here in case the UI is not available (ex. headless mode) + import omni.ui as ui + from omni.kit.viewport.utility import get_active_viewport + + # set default render mode + # note: this can be changed by calling the `set_render_mode` function + self.render_mode = self.RenderMode.FULL_RENDERING + # acquire viewport context + self._viewport_context = get_active_viewport() + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + # acquire viewport window + # TODO @mayank: Why not just use get_active_viewport_and_window() directly? + self._viewport_window = ui.Workspace.get_window("Viewport") + # counter for periodic rendering + self._render_throttle_counter = 0 + # rendering frequency in terms of number of render calls + self._render_throttle_period = 5 + + # check the case where we don't need to render the viewport + # since render_viewport can only be False in headless mode, we only need to check for offscreen_render + if not self._render_viewport and self._offscreen_render: + # disable the viewport if offscreen_render is enabled + from omni.kit.viewport.utility import get_active_viewport + + get_active_viewport().updates_enabled = False + + # override enable scene querying if rendering is enabled + # this is needed for some GUI features + if self._has_gui: + self.cfg.enable_scene_query_support = True + # set up flatcache/fabric interface (default is None) + # this is needed to flush the flatcache data into Hydra manually when calling `render()` + # ref: https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_physics.html + # note: need to do this here because super().__init__ calls render and this variable is needed + self._fabric_iface = None + # read isaac sim version (this includes build tag, release tag etc.) + # note: we do it once here because it reads the VERSION file from disk and is not expected to change. + self._isaacsim_version = get_version() + + # create a tensor for gravity + # note: this line is needed to create a "tensor" in the device to avoid issues with torch 2.1 onwards. + # the issue is with some heap memory corruption when torch tensor is created inside the asset class. + # you can reproduce the issue by commenting out this line and running the test `test_articulation.py`. + self._gravity_tensor = torch.tensor(self.cfg.gravity, dtype=torch.float32, device=self.cfg.device) + + # define a global variable to store the exceptions raised in the callback stack + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + + # add callback to deal the simulation app when simulation is stopped. + # this is needed because physics views go invalid once we stop the simulation + if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._app_control_on_stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda *args, obj=weakref.proxy(self): obj._app_control_on_stop_handle_fn(*args), + order=15, + ) + else: + self._app_control_on_stop_handle = None + self._disable_app_control_on_stop_handle = False + + # flatten out the simulation dictionary + sim_params = self.cfg.to_dict() + if sim_params is not None: + if "physx" in sim_params: + physx_params = sim_params.pop("physx") + sim_params.update(physx_params) + # create a simulation context to control the simulator + super().__init__( + stage_units_in_meters=1.0, + physics_dt=self.cfg.dt, + rendering_dt=self.cfg.dt * self.cfg.render_interval, + backend="torch", + sim_params=sim_params, + physics_prim_path=self.cfg.physics_prim_path, + device=self.cfg.device, + ) + + def _apply_physics_settings(self): + """Sets various carb physics settings.""" + # enable hydra scene-graph instancing + # note: this allows rendering of instanceable assets on the GUI + set_carb_setting(self.carb_settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking + # note: dispatcher handles how threads are launched for multi-threaded physics + set_carb_setting(self.carb_settings, "/physics/physxDispatcher", True) + # disable contact processing in omni.physx + # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. + # The physics flag gets enabled when a contact sensor is created. + if hasattr(self.cfg, "disable_contact_processing"): + omni.log.warn( + "The `disable_contact_processing` attribute is deprecated and always set to True" + " to avoid unnecessary overhead. Contact processing is automatically enabled when" + " a contact sensor is created, so manual configuration is no longer required." + ) + # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts + # are always processed. The issue is reported to the PhysX team by @mmittal. + set_carb_setting(self.carb_settings, "/physics/disableContactProcessing", True) + # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them + # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags + # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry + set_carb_setting(self.carb_settings, "/physics/collisionConeCustomGeometry", False) + set_carb_setting(self.carb_settings, "/physics/collisionCylinderCustomGeometry", False) + # hide the Simulation Settings window + set_carb_setting(self.carb_settings, "/physics/autoPopupSimulationOutputWindow", False) + + def _apply_render_settings_from_cfg(self): + """Sets rtx settings specified in the RenderCfg.""" + + # define mapping of user-friendly RenderCfg names to native carb names + rendering_setting_name_mapping = { + "enable_translucency": "/rtx/translucency/enabled", + "enable_reflections": "/rtx/reflections/enabled", + "enable_global_illumination": "/rtx/indirectDiffuse/enabled", + "enable_dlssg": "/rtx-transient/dlssg/enabled", + "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", + "dlss_mode": "/rtx/post/dlss/execMode", + "enable_direct_lighting": "/rtx/directLighting/enabled", + "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", + "enable_shadows": "/rtx/shadows/enabled", + "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", + } + + not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] + + # set preset settings (same behavior as the CLI arg --rendering_mode) + rendering_mode = self.cfg.render.rendering_mode + if rendering_mode is not None: + # check if preset is supported + supported_rendering_modes = ["performance", "balanced", "quality", "xr"] + if rendering_mode not in supported_rendering_modes: + raise ValueError( + f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." + ) + + # parse preset file + repo_path = os.path.join(carb.tokens.get_tokens_interface().resolve("${app}"), "..") + preset_filename = os.path.join(repo_path, f"apps/rendering_modes/{rendering_mode}.kit") + with open(preset_filename) as file: + preset_dict = toml.load(file) + preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) + + # set presets + for key, value in preset_dict.items(): + key = "/" + key.replace(".", "/") # convert to carb setting format + set_carb_setting(self.carb_settings, key, value) + + # set user-friendly named settings + for key, value in vars(self.cfg.render).items(): + if value is None or key in not_carb_settings: + # skip unset settings and non-carb settings + continue + if key not in rendering_setting_name_mapping: + raise ValueError( + f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" + " need to be updated." + ) + key = rendering_setting_name_mapping[key] + set_carb_setting(self.carb_settings, key, value) + + # set general carb settings + carb_settings = self.cfg.render.carb_settings + if carb_settings is not None: + for key, value in carb_settings.items(): + if "_" in key: + key = "/" + key.replace("_", "/") # convert from python variable style string + elif "." in key: + key = "/" + key.replace(".", "/") # convert from .kit file style string + if get_carb_setting(self.carb_settings, key) is None: + raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") + set_carb_setting(self.carb_settings, key, value) + + # set denoiser mode + if self.cfg.render.antialiasing_mode is not None: + try: + import omni.replicator.core as rep + + rep.settings.set_render_rtx_realtime(antialiasing=self.cfg.render.antialiasing_mode) + except Exception: + pass + + # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. + if get_carb_setting(self.carb_settings, "/rtx/rendermode").lower() == "raytracedlighting": + set_carb_setting(self.carb_settings, "/rtx/rendermode", "RaytracedLighting") + + """ + Operations - New. + """ + + def has_gui(self) -> bool: + """Returns whether the simulation has a GUI enabled. + + True if the simulation has a GUI enabled either locally or live-streamed. + """ + return self._has_gui + + def has_rtx_sensors(self) -> bool: + """Returns whether the simulation has any RTX-rendering related sensors. + + This function returns the value of the simulation parameter ``"/isaaclab/render/rtx_sensors"``. + The parameter is set to True when instances of RTX-related sensors (cameras or LiDARs) are + created using Isaac Lab's sensor classes. + + True if the simulation has RTX sensors (such as USD Cameras or LiDARs). + + For more information, please check `NVIDIA RTX documentation`_. + + .. _NVIDIA RTX documentation: https://developer.nvidia.com/rendering-technologies + """ + return self._settings.get_as_bool("/isaaclab/render/rtx_sensors") + + def is_fabric_enabled(self) -> bool: + """Returns whether the fabric interface is enabled. + + When fabric interface is enabled, USD read/write operations are disabled. Instead all applications + read and write the simulation state directly from the fabric interface. This reduces a lot of overhead + that occurs during USD read/write operations. + + For more information, please check `Fabric documentation`_. + + .. _Fabric documentation: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usd_fabric_usdrt.html + """ + return self._fabric_iface is not None + + def get_version(self) -> tuple[int, int, int]: + """Returns the version of the simulator. + + This is a wrapper around the ``isaacsim.core.version.get_version()`` function. + + The returned tuple contains the following information: + + * Major version (int): This is the year of the release (e.g. 2022). + * Minor version (int): This is the half-year of the release (e.g. 1 or 2). + * Patch version (int): This is the patch number of the release (e.g. 0). + """ + return int(self._isaacsim_version[2]), int(self._isaacsim_version[3]), int(self._isaacsim_version[4]) + + """ + Operations - New utilities. + """ + + def set_camera_view( + self, + eye: tuple[float, float, float], + target: tuple[float, float, float], + camera_prim_path: str = "/OmniverseKit_Persp", + ): + """Set the location and target of the viewport camera in the stage. + + Note: + This is a wrapper around the :math:`isaacsim.core.utils.viewports.set_camera_view` function. + It is provided here for convenience to reduce the amount of imports needed. + + Args: + eye: The location of the camera eye. + target: The location of the camera target. + camera_prim_path: The path to the camera primitive in the stage. Defaults to + "/OmniverseKit_Persp". + """ + # safe call only if we have a GUI or viewport rendering enabled + if self._has_gui or self._offscreen_render or self._render_viewport: + set_camera_view(eye, target, camera_prim_path) + + def set_render_mode(self, mode: RenderMode): + """Change the current render mode of the simulation. + + Please see :class:`RenderMode` for more information on the different render modes. + + .. note:: + When no GUI is available (locally or livestreamed), we do not need to choose whether the viewport + needs to render or not (since there is no GUI). Thus, in this case, calling the function will not + change the render mode. + + Args: + mode (RenderMode): The rendering mode. If different than SimulationContext's rendering mode, + SimulationContext's mode is changed to the new mode. + + Raises: + ValueError: If the input mode is not supported. + """ + # check if mode change is possible -- not possible when no GUI is available + if not self._has_gui: + omni.log.warn( + f"Cannot change render mode when GUI is disabled. Using the default render mode: {self.render_mode}." + ) + return + # check if there is a mode change + # note: this is mostly needed for GUI when we want to switch between full rendering and no rendering. + if mode != self.render_mode: + if mode == self.RenderMode.FULL_RENDERING: + # display the viewport and enable updates + self._viewport_context.updates_enabled = True # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = True # pyright: ignore [reportOptionalMemberAccess] + elif mode == self.RenderMode.PARTIAL_RENDERING: + # hide the viewport and disable updates + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + elif mode == self.RenderMode.NO_RENDERING: + # hide the viewport and disable updates + if self._viewport_context is not None: + self._viewport_context.updates_enabled = False # pyright: ignore [reportOptionalMemberAccess] + self._viewport_window.visible = False # pyright: ignore [reportOptionalMemberAccess] + # reset the throttle counter + self._render_throttle_counter = 0 + else: + raise ValueError(f"Unsupported render mode: {mode}! Please check `RenderMode` for details.") + # update render mode + self.render_mode = mode + + def set_setting(self, name: str, value: Any): + """Set simulation settings using the Carbonite SDK. + + .. note:: + If the input setting name does not exist, it will be created. If it does exist, the value will be + overwritten. Please make sure to use the correct setting name. + + To understand the settings interface, please refer to the + `Carbonite SDK `_ + documentation. + + Args: + name: The name of the setting. + value: The value of the setting. + """ + self._settings.set(name, value) + + def get_setting(self, name: str) -> Any: + """Read the simulation setting using the Carbonite SDK. + + Args: + name: The name of the setting. + + Returns: + The value of the setting. + """ + return self._settings.get(name) + + def forward(self) -> None: + """Updates articulation kinematics and fabric for rendering.""" + if self._fabric_iface is not None: + if self.physics_sim_view is not None and self.is_playing(): + # Update the articulations' link's poses before rendering + self.physics_sim_view.update_articulations_kinematic() + self._update_fabric(0.0, 0.0) + + """ + Operations - Override (standalone) + """ + + def reset(self, soft: bool = False): + self._disable_app_control_on_stop_handle = True + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + raise exception_to_raise + super().reset(soft=soft) + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here + if "cuda" in self.device: + torch.cuda.set_device(self.device) + # enable kinematic rendering with fabric + if self.physics_sim_view: + self.physics_sim_view._backend.initialize_kinematic_bodies() + # perform additional rendering steps to warm up replicator buffers + # this is only needed for the first time we set the simulation + if not soft: + for _ in range(2): + self.render() + self._disable_app_control_on_stop_handle = False + + def step(self, render: bool = True): + """Steps the simulation. + + .. note:: + This function blocks if the timeline is paused. It only returns when the timeline is playing. + + Args: + render: Whether to render the scene after stepping the physics simulation. + If set to False, the scene is not rendered and only the physics simulation is stepped. + """ + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + raise exception_to_raise + # check if the simulation timeline is paused. in that case keep stepping until it is playing + if not self.is_playing(): + # step the simulator (but not the physics) to have UI still active + while not self.is_playing(): + self.render() + # meantime if someone stops, break out of the loop + if self.is_stopped(): + break + # need to do one step to refresh the app + # reason: physics has to parse the scene again and inform other extensions like hydra-delegate. + # without this the app becomes unresponsive. + # FIXME: This steps physics as well, which we is not good in general. + self.app.update() + + # step the simulation + super().step(render=render) + + # app.update() may be changing the cuda device in step, so we force it back to our desired device here + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + def render(self, mode: RenderMode | None = None): + """Refreshes the rendering components including UI elements and view-ports depending on the render mode. + + This function is used to refresh the rendering components of the simulation. This includes updating the + view-ports, UI elements, and other extensions (besides physics simulation) that are running in the + background. The rendering components are refreshed based on the render mode. + + Please see :class:`RenderMode` for more information on the different render modes. + + Args: + mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. + """ + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + raise exception_to_raise + # check if we need to change the render mode + if mode is not None: + self.set_render_mode(mode) + # render based on the render mode + if self.render_mode == self.RenderMode.NO_GUI_OR_RENDERING: + # we never want to render anything here (this is for complete headless mode) + pass + elif self.render_mode == self.RenderMode.NO_RENDERING: + # throttle the rendering frequency to keep the UI responsive + self._render_throttle_counter += 1 + if self._render_throttle_counter % self._render_throttle_period == 0: + self._render_throttle_counter = 0 + # here we don't render viewport so don't need to flush fabric data + # note: we don't call super().render() anymore because they do flush the fabric data + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + else: + # manually flush the fabric data to update Hydra textures + self.forward() + # render the simulation + # note: we don't call super().render() anymore because they do above operation inside + # and we don't want to do it twice. We may remove it once we drop support for Isaac Sim 2022.2. + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + + # app.update() may be changing the cuda device, so we force it back to our desired device here + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + """ + Operations - Override (extension) + """ + + async def reset_async(self, soft: bool = False): + # need to load all "physics" information from the USD file + if not soft: + omni.physx.acquire_physx_interface().force_load_physics_from_usd() + # play the simulation + await super().reset_async(soft=soft) + + """ + Initialization/Destruction - Override. + """ + + def _init_stage(self, *args, **kwargs) -> Usd.Stage: + _ = super()._init_stage(*args, **kwargs) + # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes + # when in headless mode + self.set_setting("/app/player/playSimulations", False) + self._app.update() + self.set_setting("/app/player/playSimulations", True) + # set additional physx parameters and bind material + self._set_additional_physx_params() + # load flatcache/fabric interface + self._load_fabric_interface() + # return the stage + return self.stage + + async def _initialize_stage_async(self, *args, **kwargs) -> Usd.Stage: + await super()._initialize_stage_async(*args, **kwargs) + # set additional physx parameters and bind material + self._set_additional_physx_params() + # load flatcache/fabric interface + self._load_fabric_interface() + # return the stage + return self.stage + + @classmethod + def clear_instance(cls): + # clear the callback + if cls._instance is not None: + if cls._instance._app_control_on_stop_handle is not None: + cls._instance._app_control_on_stop_handle.unsubscribe() + cls._instance._app_control_on_stop_handle = None + # call parent to clear the instance + super().clear_instance() + + """ + Helper Functions + """ + + def _set_additional_physx_params(self): + """Sets additional PhysX parameters that are not directly supported by the parent class.""" + # obtain the physics scene api + physics_scene: UsdPhysics.Scene = self._physics_context._physics_scene + physx_scene_api: PhysxSchema.PhysxSceneAPI = self._physics_context._physx_scene_api + # assert that scene api is not None + if physx_scene_api is None: + raise RuntimeError("Physics scene API is None! Please create the scene first.") + # set parameters not directly supported by the constructor + # -- Continuous Collision Detection (CCD) + # ref: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/AdvancedCollisionDetection.html?highlight=ccd#continuous-collision-detection + self._physics_context.enable_ccd(self.cfg.physx.enable_ccd) + # -- GPU collision stack size + physx_scene_api.CreateGpuCollisionStackSizeAttr(self.cfg.physx.gpu_collision_stack_size) + # -- Improved determinism by PhysX + physx_scene_api.CreateEnableEnhancedDeterminismAttr(self.cfg.physx.enable_enhanced_determinism) + + # -- Gravity + # note: Isaac sim only takes the "up-axis" as the gravity direction. But physics allows any direction so we + # need to convert the gravity vector to a direction and magnitude pair explicitly. + gravity = np.asarray(self.cfg.gravity) + gravity_magnitude = np.linalg.norm(gravity) + + # Avoid division by zero + if gravity_magnitude != 0.0: + gravity_direction = gravity / gravity_magnitude + else: + gravity_direction = gravity + + physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) + physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) + + # position iteration count + physx_scene_api.CreateMinPositionIterationCountAttr(self.cfg.physx.min_position_iteration_count) + physx_scene_api.CreateMaxPositionIterationCountAttr(self.cfg.physx.max_position_iteration_count) + # velocity iteration count + physx_scene_api.CreateMinVelocityIterationCountAttr(self.cfg.physx.min_velocity_iteration_count) + physx_scene_api.CreateMaxVelocityIterationCountAttr(self.cfg.physx.max_velocity_iteration_count) + + # create the default physics material + # this material is used when no material is specified for a primitive + # check: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials + material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" + self.cfg.physics_material.func(material_path, self.cfg.physics_material) + # bind the physics material to the scene + bind_physics_material(self.cfg.physics_prim_path, material_path) + + def _load_fabric_interface(self): + """Loads the fabric interface if enabled.""" + if self.cfg.use_fabric: + from omni.physxfabric import get_physx_fabric_interface + + # acquire fabric interface + self._fabric_iface = get_physx_fabric_interface() + if hasattr(self._fabric_iface, "force_update"): + # The update method in the fabric interface only performs an update if a physics step has occurred. + # However, for rendering, we need to force an update since any element of the scene might have been + # modified in a reset (which occurs after the physics step) and we want the renderer to be aware of + # these changes. + self._update_fabric = self._fabric_iface.force_update + else: + # Needed for backward compatibility with older Isaac Sim versions + self._update_fabric = self._fabric_iface.update + + """ + Callbacks. + """ + + def _app_control_on_stop_handle_fn(self, event: carb.events.IEvent): + """Callback to deal with the app when the simulation is stopped. + + Once the simulation is stopped, the physics handles go invalid. After that, it is not possible to + resume the simulation from the last state. This leaves the app in an inconsistent state, where + two possible actions can be taken: + + 1. **Keep the app rendering**: In this case, the simulation is kept running and the app is not shutdown. + However, the physics is not updated and the script cannot be resumed from the last state. The + user has to manually close the app to stop the simulation. + 2. **Shutdown the app**: This is the default behavior. In this case, the app is shutdown and + the simulation is stopped. + + Note: + This callback is used only when running the simulation in a standalone python script. In an extension, + it is expected that the user handles the extension shutdown. + """ + if not self._disable_app_control_on_stop_handle: + while not omni.timeline.get_timeline_interface().is_playing(): + self.render() + return + + +@contextmanager +def build_simulation_context( + create_new_stage: bool = True, + gravity_enabled: bool = True, + device: str = "cuda:0", + dt: float = 0.01, + sim_cfg: SimulationCfg | None = None, + add_ground_plane: bool = False, + add_lighting: bool = False, + auto_add_lighting: bool = False, +) -> Iterator[SimulationContext]: + """Context manager to build a simulation context with the provided settings. + + This function facilitates the creation of a simulation context and provides flexibility in configuring various + aspects of the simulation, such as time step, gravity, device, and scene elements like ground plane and + lighting. + + If :attr:`sim_cfg` is None, then an instance of :class:`SimulationCfg` is created with default settings, with parameters + overwritten based on arguments to the function. + + An example usage of the context manager function: + + .. code-block:: python + + with build_simulation_context() as sim: + # Design the scene + + # Play the simulation + sim.reset() + while sim.is_playing(): + sim.step() + + Args: + create_new_stage: Whether to create a new stage. Defaults to True. + gravity_enabled: Whether to enable gravity in the simulation. Defaults to True. + device: Device to run the simulation on. Defaults to "cuda:0". + dt: Time step for the simulation: Defaults to 0.01. + sim_cfg: :class:`isaaclab.sim.SimulationCfg` to use for the simulation. Defaults to None. + add_ground_plane: Whether to add a ground plane to the simulation. Defaults to False. + add_lighting: Whether to add a dome light to the simulation. Defaults to False. + auto_add_lighting: Whether to automatically add a dome light to the simulation if the simulation has a GUI. + Defaults to False. This is useful for debugging tests in the GUI. + + Yields: + The simulation context to use for the simulation. + + """ + try: + if create_new_stage: + stage_utils.create_new_stage() + + if sim_cfg is None: + # Construct one and overwrite the dt, gravity, and device + sim_cfg = SimulationCfg(dt=dt) + + # Set up gravity + if gravity_enabled: + sim_cfg.gravity = (0.0, 0.0, -9.81) + else: + sim_cfg.gravity = (0.0, 0.0, 0.0) + + # Set device + sim_cfg.device = device + + # Construct simulation context + sim = SimulationContext(sim_cfg) + + if add_ground_plane: + # Ground-plane + cfg = GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + + if add_lighting or (auto_add_lighting and sim.has_gui()): + # Lighting + cfg = DomeLightCfg( + color=(0.1, 0.1, 0.1), + enable_color_temperature=True, + color_temperature=5500, + intensity=10000, + ) + # Dome light named specifically to avoid conflicts + cfg.func(prim_path="/World/defaultDomeLight", cfg=cfg, translation=(0.0, 0.0, 10.0)) + + yield sim + + except Exception: + omni.log.error(traceback.format_exc()) + raise + finally: + if not sim.has_gui(): + # Stop simulation only if we aren't rendering otherwise the app will hang indefinitely + sim.stop() + + # Clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None + raise exception_to_raise diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/__init__.py new file mode 100644 index 00000000000..2f14e712626 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/__init__.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for creating prims in Omniverse. + +Spawners are used to create prims into Omniverse simulator. At their core, they are calling the +USD Python API or Omniverse Kit Commands to create prims. However, they also provide a convenient +interface for creating prims from their respective config classes. + +There are two main ways of using the spawners: + +1. Using the function from the module + + .. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + + # spawn from USD file + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd") + prim_path = "/World/myAsset" + + # spawn using the function from the module + sim_utils.spawn_from_usd(prim_path, cfg) + +2. Using the `func` reference in the config class + + .. code-block:: python + + import isaaclab.sim as sim_utils + from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + + # spawn from USD file + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd") + prim_path = "/World/myAsset" + + # use the `func` reference in the config class + cfg.func(prim_path, cfg) + +For convenience, we recommend using the second approach, as it allows to easily change the config +class and the function call in a single line of code. + +Depending on the type of prim, the spawning-functions can also deal with the creation of prims +over multiple prim path. These need to be provided as a regex prim path expressions, which are +resolved based on the parent prim paths using the :meth:`isaaclab.sim.utils.clone` function decorator. +For example: + +* ``/World/Table_[1,2]/Robot`` will create the prims ``/World/Table_1/Robot`` and ``/World/Table_2/Robot`` + only if the parent prim ``/World/Table_1`` and ``/World/Table_2`` exist. +* ``/World/Robot_[1,2]`` will **NOT** create the prims ``/World/Robot_1`` and + ``/World/Robot_2`` as the prim path expression can be resolved to multiple prims. + +""" + +from .from_files import * # noqa: F401, F403 +from .lights import * # noqa: F401, F403 +from .materials import * # noqa: F401, F403 +from .meshes import * # noqa: F401, F403 +from .sensors import * # noqa: F401, F403 +from .shapes import * # noqa: F401, F403 +from .spawner_cfg import * # noqa: F401, F403 +from .wrappers import * # noqa: F401, F403 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/__init__.py new file mode 100644 index 00000000000..89046dd9116 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/__init__.py @@ -0,0 +1,22 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawners that spawn assets from files. + +Currently, the following spawners are supported: + +* :class:`UsdFileCfg`: Spawn an asset from a USD file. +* :class:`UrdfFileCfg`: Spawn an asset from a URDF file. +* :class:`GroundPlaneCfg`: Spawn a ground plane using the grid-world USD file. + +""" + +from .from_files import spawn_from_urdf, spawn_from_usd, spawn_ground_plane +from .from_files_cfg import GroundPlaneCfg, UrdfFileCfg, UsdFileCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files.py new file mode 100644 index 00000000000..4c56b7a270f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files.py @@ -0,0 +1,292 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils +import omni.kit.commands +import omni.log +from pxr import Gf, Sdf, Semantics, Usd + +from isaaclab.sim import converters, schemas +from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, select_usd_variants + +if TYPE_CHECKING: + from . import from_files_cfg + + +@clone +def spawn_from_usd( + prim_path: str, + cfg: from_files_cfg.UsdFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn an asset from a USD file and override the settings with the given config. + + In the case of a USD file, the asset is spawned at the default prim specified in the USD file. + If a default prim is not specified, then the asset is spawned at the root prim. + + In case a prim already exists at the given prim path, then the function does not create a new prim + or throw an error that the prim already exists. Instead, it just takes the existing prim and overrides + the settings with the given config. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which + case the translation specified in the USD file is used. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case the orientation specified in the USD file is used. + + Returns: + The prim of the spawned asset. + + Raises: + FileNotFoundError: If the USD file does not exist at the given path. + """ + # spawn asset from the given usd file + return _spawn_from_usd_file(prim_path, cfg.usd_path, cfg, translation, orientation) + + +@clone +def spawn_from_urdf( + prim_path: str, + cfg: from_files_cfg.UrdfFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn an asset from a URDF file and override the settings with the given config. + + It uses the :class:`UrdfConverter` class to create a USD file from URDF. This file is then imported + at the specified prim path. + + In case a prim already exists at the given prim path, then the function does not create a new prim + or throw an error that the prim already exists. Instead, it just takes the existing prim and overrides + the settings with the given config. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which + case the translation specified in the generated USD file is used. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case the orientation specified in the generated USD file is used. + + Returns: + The prim of the spawned asset. + + Raises: + FileNotFoundError: If the URDF file does not exist at the given path. + """ + # urdf loader to convert urdf to usd + urdf_loader = converters.UrdfConverter(cfg) + # spawn asset from the generated usd file + return _spawn_from_usd_file(prim_path, urdf_loader.usd_path, cfg, translation, orientation) + + +def spawn_ground_plane( + prim_path: str, + cfg: from_files_cfg.GroundPlaneCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawns a ground plane into the scene. + + This function loads the USD file containing the grid plane asset from Isaac Sim. It may + not work with other assets for ground planes. In those cases, please use the `spawn_from_usd` + function. + + Note: + This function takes keyword arguments to be compatible with other spawners. However, it does not + use any of the kwargs. + + Args: + prim_path: The path to spawn the asset at. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which + case the translation specified in the USD file is used. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case the orientation specified in the USD file is used. + + Returns: + The prim of the spawned asset. + + Raises: + ValueError: If the prim path already exists. + """ + # Spawn Ground-plane + if not prim_utils.is_prim_path_valid(prim_path): + prim_utils.create_prim(prim_path, usd_path=cfg.usd_path, translation=translation, orientation=orientation) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + # Create physics material + if cfg.physics_material is not None: + cfg.physics_material.func(f"{prim_path}/physicsMaterial", cfg.physics_material) + # Apply physics material to ground plane + collision_prim_path = prim_utils.get_prim_path( + prim_utils.get_first_matching_child_prim( + prim_path, predicate=lambda x: prim_utils.get_prim_type_name(x) == "Plane" + ) + ) + bind_physics_material(collision_prim_path, f"{prim_path}/physicsMaterial") + + # Scale only the mesh + # Warning: This is specific to the default grid plane asset. + if prim_utils.is_prim_path_valid(f"{prim_path}/Environment"): + # compute scale from size + scale = (cfg.size[0] / 100.0, cfg.size[1] / 100.0, 1.0) + # apply scale to the mesh + prim_utils.set_prim_property(f"{prim_path}/Environment", "xformOp:scale", scale) + + # Change the color of the plane + # Warning: This is specific to the default grid plane asset. + if cfg.color is not None: + prop_path = f"{prim_path}/Looks/theGrid/Shader.inputs:diffuse_tint" + # change the color + omni.kit.commands.execute( + "ChangePropertyCommand", + prop_path=Sdf.Path(prop_path), + value=Gf.Vec3f(*cfg.color), + prev=None, + type_to_create_if_not_exist=Sdf.ValueTypeNames.Color3f, + ) + # Remove the light from the ground plane + # It isn't bright enough and messes up with the user's lighting settings + omni.kit.commands.execute("ToggleVisibilitySelectedPrims", selected_paths=[f"{prim_path}/SphereLight"]) + + prim = prim_utils.get_prim_at_path(prim_path) + # Apply semantic tags + if hasattr(cfg, "semantic_tags") and cfg.semantic_tags is not None: + # note: taken from replicator scripts.utils.utils.py + for semantic_type, semantic_value in cfg.semantic_tags: + # deal with spaces by replacing them with underscores + semantic_type_sanitized = semantic_type.replace(" ", "_") + semantic_value_sanitized = semantic_value.replace(" ", "_") + # set the semantic API for the instance + instance_name = f"{semantic_type_sanitized}_{semantic_value_sanitized}" + sem = Semantics.SemanticsAPI.Apply(prim, instance_name) + # create semantic type and data attributes + sem.CreateSemanticTypeAttr().Set(semantic_type) + sem.CreateSemanticDataAttr().Set(semantic_value) + # return the prim + return prim + + +""" +Helper functions. +""" + + +def _spawn_from_usd_file( + prim_path: str, + usd_path: str, + cfg: from_files_cfg.FileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn an asset from a USD file and override the settings with the given config. + + In case a prim already exists at the given prim path, then the function does not create a new prim + or throw an error that the prim already exists. Instead, it just takes the existing prim and overrides + the settings with the given config. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + usd_path: The path to the USD file to spawn the asset from. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which + case the translation specified in the generated USD file is used. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case the orientation specified in the generated USD file is used. + + Returns: + The prim of the spawned asset. + + Raises: + FileNotFoundError: If the USD file does not exist at the given path. + """ + # check file path exists + stage: Usd.Stage = stage_utils.get_current_stage() + if not stage.ResolveIdentifierToEditTarget(usd_path): + raise FileNotFoundError(f"USD file not found at path: '{usd_path}'.") + # spawn asset if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + # add prim as reference to stage + prim_utils.create_prim( + prim_path, + usd_path=usd_path, + translation=translation, + orientation=orientation, + scale=cfg.scale, + ) + else: + omni.log.warn(f"A prim already exists at prim path: '{prim_path}'.") + + # modify variants + if hasattr(cfg, "variants") and cfg.variants is not None: + select_usd_variants(prim_path, cfg.variants) + + # modify rigid body properties + if cfg.rigid_props is not None: + schemas.modify_rigid_body_properties(prim_path, cfg.rigid_props) + # modify collision properties + if cfg.collision_props is not None: + schemas.modify_collision_properties(prim_path, cfg.collision_props) + # modify mass properties + if cfg.mass_props is not None: + schemas.modify_mass_properties(prim_path, cfg.mass_props) + + # modify articulation root properties + if cfg.articulation_props is not None: + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + # modify tendon properties + if cfg.fixed_tendons_props is not None: + schemas.modify_fixed_tendon_properties(prim_path, cfg.fixed_tendons_props) + # define drive API on the joints + # note: these are only for setting low-level simulation properties. all others should be set or are + # and overridden by the articulation/actuator properties. + if cfg.joint_drive_props is not None: + schemas.modify_joint_drive_properties(prim_path, cfg.joint_drive_props) + + # modify deformable body properties + if cfg.deformable_props is not None: + schemas.modify_deformable_body_properties(prim_path, cfg.deformable_props) + + # apply visual material + if cfg.visual_material is not None: + if not cfg.visual_material_path.startswith("/"): + material_path = f"{prim_path}/{cfg.visual_material_path}" + else: + material_path = cfg.visual_material_path + # create material + cfg.visual_material.func(material_path, cfg.visual_material) + # apply material + bind_visual_material(prim_path, material_path) + + # return the prim + return prim_utils.get_prim_at_path(prim_path) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files_cfg.py new file mode 100644 index 00000000000..e0621b939c6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/from_files/from_files_cfg.py @@ -0,0 +1,164 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import MISSING + +from isaaclab.sim import converters, schemas +from isaaclab.sim.spawners import materials +from isaaclab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg, SpawnerCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import from_files + + +@configclass +class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): + """Configuration parameters for spawning an asset from a file. + + This class is a base class for spawning assets from files. It includes the common parameters + for spawning assets from files, such as the path to the file and the function to use for spawning + the asset. + + Note: + By default, all properties are set to None. This means that no properties will be added or modified + to the prim outside of the properties available by default when spawning the prim. + + If they are set to a value, then the properties are modified on the spawned prim in a nested manner. + This is done by calling the respective function with the specified properties. + """ + + scale: tuple[float, float, float] | None = None + """Scale of the asset. Defaults to None, in which case the scale is not modified.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + """Properties to apply to the articulation root.""" + + fixed_tendons_props: schemas.FixedTendonsPropertiesCfg | None = None + """Properties to apply to the fixed tendons (if any).""" + + joint_drive_props: schemas.JointDrivePropertiesCfg | None = None + """Properties to apply to a joint. + + .. note:: + The joint drive properties set the USD attributes of all the joint drives in the asset. + We recommend using this attribute sparingly and only when necessary. Instead, please use the + :attr:`~isaaclab.assets.ArticulationCfg.actuators` parameter to set the joint drive properties + for specific joints in an articulation. + """ + + visual_material_path: str = "material" + """Path to the visual material to use for the prim. Defaults to "material". + + If the path is relative, then it will be relative to the prim's path. + This parameter is ignored if `visual_material` is not None. + """ + + visual_material: materials.VisualMaterialCfg | None = None + """Visual material properties to override the visual material properties in the URDF file. + + Note: + If None, then no visual material will be added. + """ + + +@configclass +class UsdFileCfg(FileCfg): + """USD file to spawn asset from. + + USD files are imported directly into the scene. However, given their complexity, there are various different + operations that can be performed on them. For example, selecting variants, applying materials, or modifying + existing properties. + + To prevent the explosion of configuration parameters, the available operations are limited to the most common + ones. These include: + + - **Selecting variants**: This is done by specifying the :attr:`variants` parameter. + - **Creating and applying materials**: This is done by specifying the :attr:`visual_material` parameter. + - **Modifying existing properties**: This is done by specifying the respective properties in the configuration + class. For instance, to modify the scale of the imported prim, set the :attr:`scale` parameter. + + See :meth:`spawn_from_usd` for more information. + + .. note:: + The configuration parameters include various properties. If not `None`, these properties + are modified on the spawned prim in a nested manner. + + If they are set to a value, then the properties are modified on the spawned prim in a nested manner. + This is done by calling the respective function with the specified properties. + """ + + func: Callable = from_files.spawn_from_usd + + usd_path: str = MISSING + """Path to the USD file to spawn asset from.""" + + variants: object | dict[str, str] | None = None + """Variants to select from in the input USD file. Defaults to None, in which case no variants are applied. + + This can either be a configclass object, in which case each attribute is used as a variant set name and its specified value, + or a dictionary mapping between the two. Please check the :meth:`~isaaclab.sim.utils.select_usd_variants` function + for more information. + """ + + +@configclass +class UrdfFileCfg(FileCfg, converters.UrdfConverterCfg): + """URDF file to spawn asset from. + + It uses the :class:`UrdfConverter` class to create a USD file from URDF and spawns the imported + USD file. Similar to the :class:`UsdFileCfg`, the generated USD file can be modified by specifying + the respective properties in the configuration class. + + See :meth:`spawn_from_urdf` for more information. + + .. note:: + The configuration parameters include various properties. If not `None`, these properties + are modified on the spawned prim in a nested manner. + + If they are set to a value, then the properties are modified on the spawned prim in a nested manner. + This is done by calling the respective function with the specified properties. + + """ + + func: Callable = from_files.spawn_from_urdf + + +""" +Spawning ground plane. +""" + + +@configclass +class GroundPlaneCfg(SpawnerCfg): + """Create a ground plane prim. + + This uses the USD for the standard grid-world ground plane from Isaac Sim by default. + """ + + func: Callable = from_files.spawn_ground_plane + + usd_path: str = f"{ISAAC_NUCLEUS_DIR}/Environments/Grid/default_environment.usd" + """Path to the USD file to spawn asset from. Defaults to the grid-world ground plane.""" + + color: tuple[float, float, float] | None = (0.0, 0.0, 0.0) + """The color of the ground plane. Defaults to (0.0, 0.0, 0.0). + + If None, then the color remains unchanged. + """ + + size: tuple[float, float] = (100.0, 100.0) + """The size of the ground plane. Defaults to 100 m x 100 m.""" + + physics_material: materials.RigidBodyMaterialCfg = materials.RigidBodyMaterialCfg() + """Physics material properties. Defaults to the default rigid body material.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/__init__.py new file mode 100644 index 00000000000..4bef8345fb8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawners that spawn lights in the simulation. + +There are various different kinds of lights that can be spawned into the USD stage. +Please check the Omniverse documentation for `lighting overview +`_. +""" + +from .lights import spawn_light +from .lights_cfg import CylinderLightCfg, DiskLightCfg, DistantLightCfg, DomeLightCfg, LightCfg, SphereLightCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights.py new file mode 100644 index 00000000000..a0c816acb1c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights.py @@ -0,0 +1,85 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +from pxr import Usd, UsdLux + +from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim + +if TYPE_CHECKING: + from . import lights_cfg + + +@clone +def spawn_light( + prim_path: str, + cfg: lights_cfg.LightCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a light prim at the specified prim path with the specified configuration. + + The created prim is based on the `USD.LuxLight `_ API. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration for the light source. + translation: The translation of the prim. Defaults to None, in which case this is set to the origin. + orientation: The orientation of the prim as (w, x, y, z). Defaults to None, in which case this + is set to identity. + + Raises: + ValueError: When a prim already exists at the specified prim path. + """ + # check if prim already exists + if prim_utils.is_prim_path_valid(prim_path): + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + # create the prim + prim = prim_utils.create_prim(prim_path, prim_type=cfg.prim_type, translation=translation, orientation=orientation) + + # convert to dict + cfg = cfg.to_dict() + # delete spawner func specific parameters + del cfg["prim_type"] + # delete custom attributes in the config that are not USD parameters + non_usd_cfg_param_names = ["func", "copy_from_source", "visible", "semantic_tags"] + for param_name in non_usd_cfg_param_names: + del cfg[param_name] + # set into USD API + for attr_name, value in cfg.items(): + # special operation for texture properties + # note: this is only used for dome light + if "texture" in attr_name: + light_prim = UsdLux.DomeLight(prim) + if attr_name == "texture_file": + light_prim.CreateTextureFileAttr(value) + elif attr_name == "texture_format": + light_prim.CreateTextureFormatAttr(value) + else: + raise ValueError(f"Unsupported texture attribute: '{attr_name}'.") + else: + if attr_name == "visible_in_primary_ray": + prim_prop_name = attr_name + else: + prim_prop_name = f"inputs:{attr_name}" + # set the attribute + safe_set_attribute_on_usd_prim(prim, prim_prop_name, value, camel_case=True) + # return the prim + return prim diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights_cfg.py new file mode 100644 index 00000000000..66aa5d2350e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/lights/lights_cfg.py @@ -0,0 +1,193 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg +from isaaclab.utils import configclass + +from . import lights + + +@configclass +class LightCfg(SpawnerCfg): + """Configuration parameters for creating a light in the scene. + + Please refer to the documentation on `USD LuxLight `_ + for more information. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + func: Callable = lights.spawn_light + + prim_type: str = MISSING + """The prim type name for the light prim.""" + + color: tuple[float, float, float] = (1.0, 1.0, 1.0) + """The color of emitted light, in energy-linear terms. Defaults to white.""" + + enable_color_temperature: bool = False + """Enables color temperature. Defaults to false.""" + + color_temperature: float = 6500.0 + """Color temperature (in Kelvin) representing the white point. The valid range is [1000, 10000]. Defaults to 6500K. + + The `color temperature `_ corresponds to the warmth + or coolness of light. Warmer light has a lower color temperature, while cooler light has a higher + color temperature. + + Note: + It only takes effect when :attr:`enable_color_temperature` is true. + """ + + normalize: bool = False + """Normalizes power by the surface area of the light. Defaults to false. + + This makes it easier to independently adjust the power and shape of the light, by causing the power + to not vary with the area or angular size of the light. + """ + + exposure: float = 0.0 + """Scales the power of the light exponentially as a power of 2. Defaults to 0.0. + + The result is multiplied against the intensity. + """ + + intensity: float = 1.0 + """Scales the power of the light linearly. Defaults to 1.0.""" + + +@configclass +class DiskLightCfg(LightCfg): + """Configuration parameters for creating a disk light in the scene. + + A disk light is a light source that emits light from a disk. It is useful for simulating + fluorescent lights. For more information, please refer to the documentation on + `USDLux DiskLight `_. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + prim_type = "DiskLight" + + radius: float = 0.5 + """Radius of the disk (in m). Defaults to 0.5m.""" + + +@configclass +class DistantLightCfg(LightCfg): + """Configuration parameters for creating a distant light in the scene. + + A distant light is a light source that is infinitely far away, and emits parallel rays of light. + It is useful for simulating sun/moon light. For more information, please refer to the documentation on + `USDLux DistantLight `_. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + prim_type = "DistantLight" + + angle: float = 0.53 + """Angular size of the light (in degrees). Defaults to 0.53 degrees. + + As an example, the Sun is approximately 0.53 degrees as seen from Earth. + Higher values broaden the light and therefore soften shadow edges. + """ + + +@configclass +class DomeLightCfg(LightCfg): + """Configuration parameters for creating a dome light in the scene. + + A dome light is a light source that emits light inwards from all directions. It is also possible to + attach a texture to the dome light, which will be used to emit light. For more information, please refer + to the documentation on `USDLux DomeLight `_. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + prim_type = "DomeLight" + + texture_file: str | None = None + """A color texture to use on the dome, such as an HDR (high dynamic range) texture intended + for IBL (image based lighting). Defaults to None. + + If None, the dome will emit a uniform color. + """ + + texture_format: Literal["automatic", "latlong", "mirroredBall", "angular", "cubeMapVerticalCross"] = "automatic" + """The parametrization format of the color map file. Defaults to "automatic". + + Valid values are: + + * ``"automatic"``: Tries to determine the layout from the file itself. For example, Renderman texture files embed an explicit parameterization. + * ``"latlong"``: Latitude as X, longitude as Y. + * ``"mirroredBall"``: An image of the environment reflected in a sphere, using an implicitly orthogonal projection. + * ``"angular"``: Similar to mirroredBall but the radial dimension is mapped linearly to the angle, providing better sampling at the edges. + * ``"cubeMapVerticalCross"``: A cube map with faces laid out as a vertical cross. + """ + + visible_in_primary_ray: bool = True + """Whether the dome light is visible in the primary ray. Defaults to True. + + If true, the texture in the sky is visible, otherwise the sky is black. + """ + + +@configclass +class CylinderLightCfg(LightCfg): + """Configuration parameters for creating a cylinder light in the scene. + + A cylinder light is a light source that emits light from a cylinder. It is useful for simulating + fluorescent lights. For more information, please refer to the documentation on + `USDLux CylinderLight `_. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + prim_type = "CylinderLight" + + length: float = 1.0 + """Length of the cylinder (in m). Defaults to 1.0m.""" + + radius: float = 0.5 + """Radius of the cylinder (in m). Defaults to 0.5m.""" + + treat_as_line: bool = False + """Treats the cylinder as a line source, i.e. a zero-radius cylinder. Defaults to false.""" + + +@configclass +class SphereLightCfg(LightCfg): + """Configuration parameters for creating a sphere light in the scene. + + A sphere light is a light source that emits light outward from a sphere. For more information, + please refer to the documentation on + `USDLux SphereLight `_. + + .. note:: + The default values for the attributes are those specified in the their official documentation. + """ + + prim_type = "SphereLight" + + radius: float = 0.5 + """Radius of the sphere. Defaults to 0.5m.""" + + treat_as_point: bool = False + """Treats the sphere as a point source, i.e. a zero-radius sphere. Defaults to false.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/__init__.py new file mode 100644 index 00000000000..8d2920aa7a5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/__init__.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawners that spawn USD-based and PhysX-based materials. + +`Materials`_ are used to define the appearance and physical properties of objects in the simulation. +In Omniverse, they are defined using NVIDIA's `Material Definition Language (MDL)`_. MDL is based on +the physically-based rendering (PBR) model, which is a set of equations that describe how light +interacts with a surface. The PBR model is used to create realistic-looking materials. + +While MDL is primarily used for defining the appearance of objects, it can be extended to define +the physical properties of objects. For example, the friction and restitution coefficients of a +rubber material. A `physics material`_ can be assigned to a physics object to +define its physical properties. There are different kinds of physics materials, such as rigid body +material, deformable material, and fluid material. + +In order to apply a material to an object, we "bind" the geometry of the object to the material. +For this, we use the `USD Material Binding API`_. The material binding API takes in the path to +the geometry and the path to the material, and binds them together. + +For physics material, the material is bound to the physics object with the 'physics' purpose. +When parsing physics material properties on an object, the following priority is used: + +1. Material binding with a 'physics' purpose (physics material) +2. Material binding with no purpose (visual material) +3. Material binding with a 'physics' purpose on the `Physics Scene`_ prim. +4. Default values of material properties inside PhysX. + +Usage: + .. code-block:: python + + import isaacsim.core.utils.prims as prim_utils + + import isaaclab.sim as sim_utils + + # create a visual material + visual_material_cfg = sim_utils.GlassMdlCfg(glass_ior=1.0, thin_walled=True) + visual_material_cfg.func("/World/Looks/glassMaterial", visual_material_cfg) + + # create a mesh prim + cube_cfg = sim_utils.CubeCfg(size=[1.0, 1.0, 1.0]) + cube_cfg.func("/World/Primitives/Cube", cube_cfg) + + # bind the cube to the visual material + sim_utils.bind_visual_material("/World/Primitives/Cube", "/World/Looks/glassMaterial") + + +.. _Material Definition Language (MDL): https://raytracing-docs.nvidia.com/mdl/introduction/index.html#mdl_introduction# +.. _Materials: https://docs.omniverse.nvidia.com/materials-and-rendering/latest/materials.html +.. _physics material: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials +.. _USD Material Binding API: https://openusd.org/dev/api/class_usd_shade_material_binding_a_p_i.html +.. _Physics Scene: https://openusd.org/dev/api/usd_physics_page_front.html +""" + +from .physics_materials import spawn_deformable_body_material, spawn_rigid_body_material +from .physics_materials_cfg import DeformableBodyMaterialCfg, PhysicsMaterialCfg, RigidBodyMaterialCfg +from .visual_materials import spawn_from_mdl_file, spawn_preview_surface +from .visual_materials_cfg import GlassMdlCfg, MdlFileCfg, PreviewSurfaceCfg, VisualMaterialCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials.py new file mode 100644 index 00000000000..afc983d2289 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials.py @@ -0,0 +1,128 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils +from pxr import PhysxSchema, Usd, UsdPhysics, UsdShade + +from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_schema + +if TYPE_CHECKING: + from . import physics_materials_cfg + + +@clone +def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBodyMaterialCfg) -> Usd.Prim: + """Create material with rigid-body physics properties. + + Rigid body materials are used to define the physical properties to meshes of a rigid body. These + include the friction, restitution, and their respective combination modes. For more information on + rigid body material, please refer to the `documentation on PxMaterial `_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration for the physics material. + + Returns: + The spawned rigid body material prim. + + Raises: + ValueError: When a prim already exists at the specified prim path and is not a material. + """ + # create material prim if no prim exists + if not prim_utils.is_prim_path_valid(prim_path): + _ = UsdShade.Material.Define(stage_utils.get_current_stage(), prim_path) + + # obtain prim + prim = prim_utils.get_prim_at_path(prim_path) + # check if prim is a material + if not prim.IsA(UsdShade.Material): + raise ValueError(f"A prim already exists at path: '{prim_path}' but is not a material.") + # retrieve the USD rigid-body api + usd_physics_material_api = UsdPhysics.MaterialAPI(prim) + if not usd_physics_material_api: + usd_physics_material_api = UsdPhysics.MaterialAPI.Apply(prim) + # retrieve the collision api + physx_material_api = PhysxSchema.PhysxMaterialAPI(prim) + if not physx_material_api: + physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into USD API + for attr_name in ["static_friction", "dynamic_friction", "restitution"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_physics_material_api, attr_name, value, camel_case=True) + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_material_api, attr_name, value, camel_case=True) + # return the prim + return prim + + +@clone +def spawn_deformable_body_material(prim_path: str, cfg: physics_materials_cfg.DeformableBodyMaterialCfg) -> Usd.Prim: + """Create material with deformable-body physics properties. + + Deformable body materials are used to define the physical properties to meshes of a deformable body. These + include the friction and deformable body properties. For more information on deformable body material, + please refer to the documentation on `PxFEMSoftBodyMaterial`_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration for the physics material. + + Returns: + The spawned deformable body material prim. + + Raises: + ValueError: When a prim already exists at the specified prim path and is not a material. + + .. _PxFEMSoftBodyMaterial: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/structPxFEMSoftBodyMaterialModel.html + """ + # create material prim if no prim exists + if not prim_utils.is_prim_path_valid(prim_path): + _ = UsdShade.Material.Define(stage_utils.get_current_stage(), prim_path) + + # obtain prim + prim = prim_utils.get_prim_at_path(prim_path) + # check if prim is a material + if not prim.IsA(UsdShade.Material): + raise ValueError(f"A prim already exists at path: '{prim_path}' but is not a material.") + # retrieve the deformable-body api + physx_deformable_body_material_api = PhysxSchema.PhysxDeformableBodyMaterialAPI(prim) + if not physx_deformable_body_material_api: + physx_deformable_body_material_api = PhysxSchema.PhysxDeformableBodyMaterialAPI.Apply(prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_deformable_body_material_api, attr_name, value, camel_case=True) + # return the prim + return prim diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials_cfg.py new file mode 100644 index 00000000000..4b00111129f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/physics_materials_cfg.py @@ -0,0 +1,135 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import physics_materials + + +@configclass +class PhysicsMaterialCfg: + """Configuration parameters for creating a physics material. + + Physics material are PhysX schemas that can be applied to a USD material prim to define the + physical properties related to the material. For example, the friction coefficient, restitution + coefficient, etc. For more information on physics material, please refer to the + `PhysX documentation `__. + """ + + func: Callable = MISSING + """Function to use for creating the material.""" + + +@configclass +class RigidBodyMaterialCfg(PhysicsMaterialCfg): + """Physics material parameters for rigid bodies. + + See :meth:`spawn_rigid_body_material` for more information. + + Note: + The default values are the `default values used by PhysX 5 + `__. + """ + + func: Callable = physics_materials.spawn_rigid_body_material + + static_friction: float = 0.5 + """The static friction coefficient. Defaults to 0.5.""" + + dynamic_friction: float = 0.5 + """The dynamic friction coefficient. Defaults to 0.5.""" + + restitution: float = 0.0 + """The restitution coefficient. Defaults to 0.0.""" + + improve_patch_friction: bool = True + """Whether to enable patch friction. Defaults to True.""" + + friction_combine_mode: Literal["average", "min", "multiply", "max"] = "average" + """Determines the way friction will be combined during collisions. Defaults to `"average"`. + + .. attention:: + + When two physics materials with different combine modes collide, the combine mode with the higher + priority will be used. The priority order is provided `here + `__. + """ + + restitution_combine_mode: Literal["average", "min", "multiply", "max"] = "average" + """Determines the way restitution coefficient will be combined during collisions. Defaults to `"average"`. + + .. attention:: + + When two physics materials with different combine modes collide, the combine mode with the higher + priority will be used. The priority order is provided `here + `__. + """ + + compliant_contact_stiffness: float = 0.0 + """Spring stiffness for a compliant contact model using implicit springs. Defaults to 0.0. + + A higher stiffness results in behavior closer to a rigid contact. The compliant contact model is only enabled + if the stiffness is larger than 0. + """ + + compliant_contact_damping: float = 0.0 + """Damping coefficient for a compliant contact model using implicit springs. Defaults to 0.0. + + Irrelevant if compliant contacts are disabled when :obj:`compliant_contact_stiffness` is set to zero and + rigid contacts are active. + """ + + +@configclass +class DeformableBodyMaterialCfg(PhysicsMaterialCfg): + """Physics material parameters for deformable bodies. + + See :meth:`spawn_deformable_body_material` for more information. + + Note: + The default values are the `default values used by PhysX 5 + `__. + """ + + func: Callable = physics_materials.spawn_deformable_body_material + + density: float | None = None + """The material density. Defaults to None, in which case the simulation decides the default density.""" + + dynamic_friction: float = 0.25 + """The dynamic friction. Defaults to 0.25.""" + + youngs_modulus: float = 50000000.0 + """The Young's modulus, which defines the body's stiffness. Defaults to 50000000.0. + + The Young's modulus is a measure of the material's ability to deform under stress. It is measured in Pascals (Pa). + """ + + poissons_ratio: float = 0.45 + """The Poisson's ratio which defines the body's volume preservation. Defaults to 0.45. + + The Poisson's ratio is a measure of the material's ability to expand in the lateral direction when compressed + in the axial direction. It is a dimensionless number between 0 and 0.5. Using a value of 0.5 will make the + material incompressible. + """ + + elasticity_damping: float = 0.005 + """The elasticity damping for the deformable material. Defaults to 0.005.""" + + damping_scale: float = 1.0 + """The damping scale for the deformable material. Defaults to 1.0. + + A scale of 1 corresponds to default damping. A value of 0 will only apply damping to certain motions leading + to special effects that look similar to water filled soft bodies. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials.py new file mode 100644 index 00000000000..237bd0f2d54 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials.py @@ -0,0 +1,121 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +import omni.kit.commands +from pxr import Usd + +from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim +from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR + +if TYPE_CHECKING: + from . import visual_materials_cfg + + +@clone +def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfaceCfg) -> Usd.Prim: + """Create a preview surface prim and override the settings with the given config. + + A preview surface is a physically-based surface that handles simple shaders while supporting + both *specular* and *metallic* workflows. All color inputs are in linear color space (RGB). + For more information, see the `documentation `__. + + The function calls the USD command `CreatePreviewSurfaceMaterialPrim`_ to create the prim. + + .. _CreatePreviewSurfaceMaterialPrim: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreatePreviewSurfaceMaterialPrimCommand.html + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn material if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + omni.kit.commands.execute("CreatePreviewSurfaceMaterialPrim", mtl_path=prim_path, select_new_prim=False) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + # obtain prim + prim = prim_utils.get_prim_at_path(f"{prim_path}/Shader") + # apply properties + cfg = cfg.to_dict() + del cfg["func"] + for attr_name, attr_value in cfg.items(): + safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=True) + # return prim + return prim + + +@clone +def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg) -> Usd.Prim: + """Load a material from its MDL file and override the settings with the given config. + + NVIDIA's `Material Definition Language (MDL) `__ + is a language for defining physically-based materials. The MDL file format is a binary format + that can be loaded by Omniverse and other applications such as Adobe Substance Designer. + To learn more about MDL, see the `documentation `_. + + The function calls the USD command `CreateMdlMaterialPrim`_ to create the prim. + + .. _CreateMdlMaterialPrim: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateMdlMaterialPrimCommand.html + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn material if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + # extract material name from path + material_name = cfg.mdl_path.split("/")[-1].split(".")[0] + omni.kit.commands.execute( + "CreateMdlMaterialPrim", + mtl_url=cfg.mdl_path.format(NVIDIA_NUCLEUS_DIR=NVIDIA_NUCLEUS_DIR), + mtl_name=material_name, + mtl_path=prim_path, + select_new_prim=False, + ) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + # obtain prim + prim = prim_utils.get_prim_at_path(f"{prim_path}/Shader") + # apply properties + cfg = cfg.to_dict() + del cfg["func"] + del cfg["mdl_path"] + for attr_name, attr_value in cfg.items(): + safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=False) + # return prim + return prim diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials_cfg.py new file mode 100644 index 00000000000..777593059b5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/materials/visual_materials_cfg.py @@ -0,0 +1,115 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Callable +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from . import visual_materials + + +@configclass +class VisualMaterialCfg: + """Configuration parameters for creating a visual material.""" + + func: Callable = MISSING + """The function to use for creating the material.""" + + +@configclass +class PreviewSurfaceCfg(VisualMaterialCfg): + """Configuration parameters for creating a preview surface. + + See :meth:`spawn_preview_surface` for more information. + """ + + func: Callable = visual_materials.spawn_preview_surface + + diffuse_color: tuple[float, float, float] = (0.18, 0.18, 0.18) + """The RGB diffusion color. This is the base color of the surface. Defaults to a dark gray.""" + emissive_color: tuple[float, float, float] = (0.0, 0.0, 0.0) + """The RGB emission component of the surface. Defaults to black.""" + roughness: float = 0.5 + """The roughness for specular lobe. Ranges from 0 (smooth) to 1 (rough). Defaults to 0.5.""" + metallic: float = 0.0 + """The metallic component. Ranges from 0 (dielectric) to 1 (metal). Defaults to 0.""" + opacity: float = 1.0 + """The opacity of the surface. Ranges from 0 (transparent) to 1 (opaque). Defaults to 1. + + Note: + Opacity only affects the surface's appearance during interactive rendering. + """ + + +@configclass +class MdlFileCfg(VisualMaterialCfg): + """Configuration parameters for loading an MDL material from a file. + + See :meth:`spawn_from_mdl_file` for more information. + """ + + func: Callable = visual_materials.spawn_from_mdl_file + + mdl_path: str = MISSING + """The path to the MDL material. + + NVIDIA Omniverse provides various MDL materials in the NVIDIA Nucleus. + To use these materials, you can set the path of the material in the nucleus directory + using the ``{NVIDIA_NUCLEUS_DIR}`` variable. This is internally resolved to the path of the + NVIDIA Nucleus directory on the host machine through the attribute + :attr:`isaaclab.utils.assets.NVIDIA_NUCLEUS_DIR`. + + For example, to use the "Aluminum_Anodized" material, you can set the path to: + ``{NVIDIA_NUCLEUS_DIR}/Materials/Base/Metals/Aluminum_Anodized.mdl``. + """ + project_uvw: bool | None = None + """Whether to project the UVW coordinates of the material. Defaults to None. + + If None, then the default setting in the MDL material will be used. + """ + albedo_brightness: float | None = None + """Multiplier for the diffuse color of the material. Defaults to None. + + If None, then the default setting in the MDL material will be used. + """ + texture_scale: tuple[float, float] | None = None + """The scale of the texture. Defaults to None. + + If None, then the default setting in the MDL material will be used. + """ + + +@configclass +class GlassMdlCfg(VisualMaterialCfg): + """Configuration parameters for loading a glass MDL material. + + This is a convenience class for loading a glass MDL material. For more information on + glass materials, see the `documentation `__. + + .. note:: + The default values are taken from the glass material in the NVIDIA Nucleus. + """ + + func: Callable = visual_materials.spawn_from_mdl_file + + mdl_path: str = "OmniGlass.mdl" + """The path to the MDL material. Defaults to the glass material in the NVIDIA Nucleus.""" + glass_color: tuple[float, float, float] = (1.0, 1.0, 1.0) + """The RGB color or tint of the glass. Defaults to white.""" + frosting_roughness: float = 0.0 + """The amount of reflectivity of the surface. Ranges from 0 (perfectly clear) to 1 (frosted). + Defaults to 0.""" + thin_walled: bool = False + """Whether to perform thin-walled refraction. Defaults to False.""" + glass_ior: float = 1.491 + """The incidence of refraction to control how much light is bent when passing through the glass. + Defaults to 1.491, which is the IOR of glass. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/__init__.py new file mode 100644 index 00000000000..f272f38e0e1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/__init__.py @@ -0,0 +1,29 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawning meshes in the simulation. + +NVIDIA Omniverse deals with meshes as `USDGeomMesh`_ prims. This sub-module provides various +configurations to spawn different types of meshes. Based on the configuration, the spawned prim can be: + +* a visual mesh (no physics) +* a static collider (no rigid or deformable body) +* a deformable body (with deformable properties) + +.. note:: + While rigid body properties can be set on a mesh, it is recommended to use the + :mod:`isaaclab.sim.spawners.shapes` module to spawn rigid bodies. This is because USD shapes + are more optimized for physics simulations. + +.. _USDGeomMesh: https://openusd.org/release/api/class_usd_geom_mesh.html +""" + +from .meshes import spawn_mesh_capsule, spawn_mesh_cone, spawn_mesh_cuboid, spawn_mesh_cylinder, spawn_mesh_sphere +from .meshes_cfg import MeshCapsuleCfg, MeshCfg, MeshConeCfg, MeshCuboidCfg, MeshCylinderCfg, MeshSphereCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes.py new file mode 100644 index 00000000000..bd0ee727099 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes.py @@ -0,0 +1,369 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import trimesh +import trimesh.transformations +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +from pxr import Usd, UsdPhysics + +from isaaclab.sim import schemas +from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone + +from ..materials import DeformableBodyMaterialCfg, RigidBodyMaterialCfg + +if TYPE_CHECKING: + from . import meshes_cfg + + +@clone +def spawn_mesh_sphere( + prim_path: str, + cfg: meshes_cfg.MeshSphereCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD-Mesh sphere prim with the given attributes. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # create a trimesh sphere + sphere = trimesh.creation.uv_sphere(radius=cfg.radius) + # spawn the sphere as a mesh + _spawn_mesh_geom_from_mesh(prim_path, cfg, sphere, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_mesh_cuboid( + prim_path: str, + cfg: meshes_cfg.MeshCuboidCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD-Mesh cuboid prim with the given attributes. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ # create a trimesh box + box = trimesh.creation.box(cfg.size) + # spawn the cuboid as a mesh + _spawn_mesh_geom_from_mesh(prim_path, cfg, box, translation, orientation, None) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_mesh_cylinder( + prim_path: str, + cfg: meshes_cfg.MeshCylinderCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD-Mesh cylinder prim with the given attributes. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # align axis from "Z" to input by rotating the cylinder + axis = cfg.axis.upper() + if axis == "X": + transform = trimesh.transformations.rotation_matrix(np.pi / 2, [0, 1, 0]) + elif axis == "Y": + transform = trimesh.transformations.rotation_matrix(-np.pi / 2, [1, 0, 0]) + else: + transform = None + # create a trimesh cylinder + cylinder = trimesh.creation.cylinder(radius=cfg.radius, height=cfg.height, transform=transform) + # spawn the cylinder as a mesh + _spawn_mesh_geom_from_mesh(prim_path, cfg, cylinder, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_mesh_capsule( + prim_path: str, + cfg: meshes_cfg.MeshCapsuleCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD-Mesh capsule prim with the given attributes. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # align axis from "Z" to input by rotating the cylinder + axis = cfg.axis.upper() + if axis == "X": + transform = trimesh.transformations.rotation_matrix(np.pi / 2, [0, 1, 0]) + elif axis == "Y": + transform = trimesh.transformations.rotation_matrix(-np.pi / 2, [1, 0, 0]) + else: + transform = None + # create a trimesh capsule + capsule = trimesh.creation.capsule(radius=cfg.radius, height=cfg.height, transform=transform) + # spawn capsule if it doesn't exist. + _spawn_mesh_geom_from_mesh(prim_path, cfg, capsule, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_mesh_cone( + prim_path: str, + cfg: meshes_cfg.MeshConeCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD-Mesh cone prim with the given attributes. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # align axis from "Z" to input by rotating the cylinder + axis = cfg.axis.upper() + if axis == "X": + transform = trimesh.transformations.rotation_matrix(np.pi / 2, [0, 1, 0]) + elif axis == "Y": + transform = trimesh.transformations.rotation_matrix(-np.pi / 2, [1, 0, 0]) + else: + transform = None + # create a trimesh cone + cone = trimesh.creation.cone(radius=cfg.radius, height=cfg.height, transform=transform) + # spawn cone if it doesn't exist. + _spawn_mesh_geom_from_mesh(prim_path, cfg, cone, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +""" +Helper functions. +""" + + +def _spawn_mesh_geom_from_mesh( + prim_path: str, + cfg: meshes_cfg.MeshCfg, + mesh: trimesh.Trimesh, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + scale: tuple[float, float, float] | None = None, +): + """Create a `USDGeomMesh`_ prim from the given mesh. + + This function is similar to :func:`shapes._spawn_geom_from_prim_type` but spawns the prim from a given mesh. + In case of the mesh, it is spawned as a USDGeomMesh prim with the given vertices and faces. + + There is a difference in how the properties are applied to the prim based on the type of object: + + - Deformable body properties: The properties are applied to the mesh prim: ``{prim_path}/geometry/mesh``. + - Collision properties: The properties are applied to the mesh prim: ``{prim_path}/geometry/mesh``. + - Rigid body properties: The properties are applied to the parent prim: ``{prim_path}``. + + Args: + prim_path: The prim path to spawn the asset at. + cfg: The config containing the properties to apply. + mesh: The mesh to spawn the prim from. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + scale: The scale to apply to the prim. Defaults to None, in which case this is set to identity. + + Raises: + ValueError: If a prim already exists at the given path. + ValueError: If both deformable and rigid properties are used. + ValueError: If both deformable and collision properties are used. + ValueError: If the physics material is not of the correct type. Deformable properties require a deformable + physics material, and rigid properties require a rigid physics material. + + .. _USDGeomMesh: https://openusd.org/dev/api/class_usd_geom_mesh.html + """ + # spawn geometry if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + prim_utils.create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + # check that invalid schema types are not used + if cfg.deformable_props is not None and cfg.rigid_props is not None: + raise ValueError("Cannot use both deformable and rigid properties at the same time.") + if cfg.deformable_props is not None and cfg.collision_props is not None: + raise ValueError("Cannot use both deformable and collision properties at the same time.") + # check material types are correct + if cfg.deformable_props is not None and cfg.physics_material is not None: + if not isinstance(cfg.physics_material, DeformableBodyMaterialCfg): + raise ValueError("Deformable properties require a deformable physics material.") + if cfg.rigid_props is not None and cfg.physics_material is not None: + if not isinstance(cfg.physics_material, RigidBodyMaterialCfg): + raise ValueError("Rigid properties require a rigid physics material.") + + # create all the paths we need for clarity + geom_prim_path = prim_path + "/geometry" + mesh_prim_path = geom_prim_path + "/mesh" + + # create the mesh prim + mesh_prim = prim_utils.create_prim( + mesh_prim_path, + prim_type="Mesh", + scale=scale, + attributes={ + "points": mesh.vertices, + "faceVertexIndices": mesh.faces.flatten(), + "faceVertexCounts": np.asarray([3] * len(mesh.faces)), + "subdivisionScheme": "bilinear", + }, + ) + + # note: in case of deformable objects, we need to apply the deformable properties to the mesh prim. + # this is different from rigid objects where we apply the properties to the parent prim. + if cfg.deformable_props is not None: + # apply mass properties + if cfg.mass_props is not None: + schemas.define_mass_properties(mesh_prim_path, cfg.mass_props) + # apply deformable body properties + schemas.define_deformable_body_properties(mesh_prim_path, cfg.deformable_props) + elif cfg.collision_props is not None: + # decide on type of collision approximation based on the mesh + if cfg.__class__.__name__ == "MeshSphereCfg": + collision_approximation = "boundingSphere" + elif cfg.__class__.__name__ == "MeshCuboidCfg": + collision_approximation = "boundingCube" + else: + # for: MeshCylinderCfg, MeshCapsuleCfg, MeshConeCfg + collision_approximation = "convexHull" + # apply collision approximation to mesh + # note: for primitives, we use the convex hull approximation -- this should be sufficient for most cases. + mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(mesh_prim) + mesh_collision_api.GetApproximationAttr().Set(collision_approximation) + # apply collision properties + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) + + # apply visual material + if cfg.visual_material is not None: + if not cfg.visual_material_path.startswith("/"): + material_path = f"{geom_prim_path}/{cfg.visual_material_path}" + else: + material_path = cfg.visual_material_path + # create material + cfg.visual_material.func(material_path, cfg.visual_material) + # apply material + bind_visual_material(mesh_prim_path, material_path) + + # apply physics material + if cfg.physics_material is not None: + if not cfg.physics_material_path.startswith("/"): + material_path = f"{geom_prim_path}/{cfg.physics_material_path}" + else: + material_path = cfg.physics_material_path + # create material + cfg.physics_material.func(material_path, cfg.physics_material) + # apply material + bind_physics_material(mesh_prim_path, material_path) + + # note: we apply the rigid properties to the parent prim in case of rigid objects. + if cfg.rigid_props is not None: + # apply mass properties + if cfg.mass_props is not None: + schemas.define_mass_properties(prim_path, cfg.mass_props) + # apply rigid properties + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes_cfg.py new file mode 100644 index 00000000000..14b9a3c3d28 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/meshes/meshes_cfg.py @@ -0,0 +1,149 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim.spawners import materials +from isaaclab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg +from isaaclab.utils import configclass + +from . import meshes + + +@configclass +class MeshCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): + """Configuration parameters for a USD Geometry or Geom prim. + + This class is similar to :class:`ShapeCfg` but is specifically for meshes. + + Meshes support both rigid and deformable properties. However, their schemas are applied at + different levels in the USD hierarchy based on the type of the object. These are described below: + + - Deformable body properties: Applied to the mesh prim: ``{prim_path}/geometry/mesh``. + - Collision properties: Applied to the mesh prim: ``{prim_path}/geometry/mesh``. + - Rigid body properties: Applied to the parent prim: ``{prim_path}``. + + where ``{prim_path}`` is the path to the prim in the USD stage and ``{prim_path}/geometry/mesh`` + is the path to the mesh prim. + + .. note:: + There are mututally exclusive parameters for rigid and deformable properties. If both are set, + then an error will be raised. This also holds if collision and deformable properties are set together. + + """ + + visual_material_path: str = "material" + """Path to the visual material to use for the prim. Defaults to "material". + + If the path is relative, then it will be relative to the prim's path. + This parameter is ignored if `visual_material` is not None. + """ + + visual_material: materials.VisualMaterialCfg | None = None + """Visual material properties. + + Note: + If None, then no visual material will be added. + """ + + physics_material_path: str = "material" + """Path to the physics material to use for the prim. Defaults to "material". + + If the path is relative, then it will be relative to the prim's path. + This parameter is ignored if `physics_material` is not None. + """ + + physics_material: materials.PhysicsMaterialCfg | None = None + """Physics material properties. + + Note: + If None, then no physics material will be added. + """ + + +@configclass +class MeshSphereCfg(MeshCfg): + """Configuration parameters for a sphere mesh prim with deformable properties. + + See :meth:`spawn_mesh_sphere` for more information. + """ + + func: Callable = meshes.spawn_mesh_sphere + + radius: float = MISSING + """Radius of the sphere (in m).""" + + +@configclass +class MeshCuboidCfg(MeshCfg): + """Configuration parameters for a cuboid mesh prim with deformable properties. + + See :meth:`spawn_mesh_cuboid` for more information. + """ + + func: Callable = meshes.spawn_mesh_cuboid + + size: tuple[float, float, float] = MISSING + """Size of the cuboid (in m).""" + + +@configclass +class MeshCylinderCfg(MeshCfg): + """Configuration parameters for a cylinder mesh prim with deformable properties. + + See :meth:`spawn_cylinder` for more information. + """ + + func: Callable = meshes.spawn_mesh_cylinder + + radius: float = MISSING + """Radius of the cylinder (in m).""" + height: float = MISSING + """Height of the cylinder (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the cylinder. Defaults to "Z".""" + + +@configclass +class MeshCapsuleCfg(MeshCfg): + """Configuration parameters for a capsule mesh prim. + + See :meth:`spawn_capsule` for more information. + """ + + func: Callable = meshes.spawn_mesh_capsule + + radius: float = MISSING + """Radius of the capsule (in m).""" + height: float = MISSING + """Height of the capsule (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the capsule. Defaults to "Z".""" + + +@configclass +class MeshConeCfg(MeshCfg): + """Configuration parameters for a cone mesh prim. + + See :meth:`spawn_cone` for more information. + """ + + func: Callable = meshes.spawn_mesh_cone + + radius: float = MISSING + """Radius of the cone (in m).""" + height: float = MISSING + """Height of the v (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the cone. Defaults to "Z".""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/__init__.py new file mode 100644 index 00000000000..9512b6d3c11 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/__init__.py @@ -0,0 +1,20 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawners that spawn sensors in the simulation. + +Currently, the following sensors are supported: + +* Camera: A USD camera prim with settings for pinhole or fisheye projections. + +""" + +from .sensors import spawn_camera +from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors.py new file mode 100644 index 00000000000..aaff891851f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors.py @@ -0,0 +1,147 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +import omni.kit.commands +import omni.log +from pxr import Sdf, Usd + +from isaaclab.sim.utils import clone +from isaaclab.utils import to_camel_case + +if TYPE_CHECKING: + from . import sensors_cfg + + +CUSTOM_PINHOLE_CAMERA_ATTRIBUTES = { + "projection_type": ("cameraProjectionType", Sdf.ValueTypeNames.Token), +} +"""Custom attributes for pinhole camera model. + +The dictionary maps the attribute name in the configuration to the attribute name in the USD prim. +""" + + +CUSTOM_FISHEYE_CAMERA_ATTRIBUTES = { + "projection_type": ("cameraProjectionType", Sdf.ValueTypeNames.Token), + "fisheye_nominal_width": ("fthetaWidth", Sdf.ValueTypeNames.Float), + "fisheye_nominal_height": ("fthetaHeight", Sdf.ValueTypeNames.Float), + "fisheye_optical_centre_x": ("fthetaCx", Sdf.ValueTypeNames.Float), + "fisheye_optical_centre_y": ("fthetaCy", Sdf.ValueTypeNames.Float), + "fisheye_max_fov": ("fthetaMaxFov", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_a": ("fthetaPolyA", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_b": ("fthetaPolyB", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_c": ("fthetaPolyC", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_d": ("fthetaPolyD", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_e": ("fthetaPolyE", Sdf.ValueTypeNames.Float), + "fisheye_polynomial_f": ("fthetaPolyF", Sdf.ValueTypeNames.Float), +} +"""Custom attributes for fisheye camera model. + +The dictionary maps the attribute name in the configuration to the attribute name in the USD prim. +""" + + +@clone +def spawn_camera( + prim_path: str, + cfg: sensors_cfg.PinholeCameraCfg | sensors_cfg.FisheyeCameraCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USD camera prim with given projection type. + + The function creates various attributes on the camera prim that specify the camera's properties. + These are later used by ``omni.replicator.core`` to render the scene with the given camera. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn camera if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + prim_utils.create_prim(prim_path, "Camera", translation=translation, orientation=orientation) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + # lock camera from viewport (this disables viewport movement for camera) + if cfg.lock_camera: + omni.kit.commands.execute( + "ChangePropertyCommand", + prop_path=Sdf.Path(f"{prim_path}.omni:kit:cameraLock"), + value=True, + prev=None, + type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, + ) + # decide the custom attributes to add + if cfg.projection_type == "pinhole": + attribute_types = CUSTOM_PINHOLE_CAMERA_ATTRIBUTES + else: + attribute_types = CUSTOM_FISHEYE_CAMERA_ATTRIBUTES + + # TODO: Adjust to handle aperture offsets once supported by omniverse + # Internal ticket from rendering team: OM-42611 + if cfg.horizontal_aperture_offset > 1e-4 or cfg.vertical_aperture_offset > 1e-4: + omni.log.warn("Camera aperture offsets are not supported by Omniverse. These parameters will be ignored.") + + # custom attributes in the config that are not USD Camera parameters + non_usd_cfg_param_names = [ + "func", + "copy_from_source", + "lock_camera", + "visible", + "semantic_tags", + "from_intrinsic_matrix", + ] + # get camera prim + prim = prim_utils.get_prim_at_path(prim_path) + # create attributes for the fisheye camera model + # note: for pinhole those are already part of the USD camera prim + for attr_name, attr_type in attribute_types.values(): + # check if attribute does not exist + if prim.GetAttribute(attr_name).Get() is None: + # create attribute based on type + prim.CreateAttribute(attr_name, attr_type) + # set attribute values + for param_name, param_value in cfg.__dict__.items(): + # check if value is valid + if param_value is None or param_name in non_usd_cfg_param_names: + continue + # obtain prim property name + if param_name in attribute_types: + # check custom attributes + prim_prop_name = attribute_types[param_name][0] + else: + # convert attribute name in prim to cfg name + prim_prop_name = to_camel_case(param_name, to="cC") + # get attribute from the class + prim.GetAttribute(prim_prop_name).Set(param_value) + # return the prim + return prim_utils.get_prim_at_path(prim_path) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors_cfg.py new file mode 100644 index 00000000000..4adce34fde0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -0,0 +1,235 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Callable +from typing import Literal + +import isaaclab.utils.sensors as sensor_utils +from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg +from isaaclab.utils import configclass + +from . import sensors + + +@configclass +class PinholeCameraCfg(SpawnerCfg): + """Configuration parameters for a USD camera prim with pinhole camera settings. + + For more information on the parameters, please refer to the `camera documentation `__. + + ..note :: + Focal length as well as the aperture sizes and offsets are set as a tenth of the world unit. In our case, the + world unit is Meter s.t. all of these values are set in cm. + + .. note:: + The default values are taken from the `Replicator camera `__ + function. + """ + + func: Callable = sensors.spawn_camera + + projection_type: str = "pinhole" + """Type of projection to use for the camera. Defaults to "pinhole". + + Note: + Currently only "pinhole" is supported. + """ + + clipping_range: tuple[float, float] = (0.01, 1e6) + """Near and far clipping distances (in m). Defaults to (0.01, 1e6). + + The minimum clipping range will shift the camera forward by the specified distance. Don't set it too high to + avoid issues for distance related data types (e.g., ``distance_to_image_plane``). + """ + + focal_length: float = 24.0 + """Perspective focal length (in cm). Defaults to 24.0cm. + + Longer lens lengths narrower FOV, shorter lens lengths wider FOV. + """ + + focus_distance: float = 400.0 + """Distance from the camera to the focus plane (in m). Defaults to 400.0. + + The distance at which perfect sharpness is achieved. + """ + + f_stop: float = 0.0 + """Lens aperture. Defaults to 0.0, which turns off focusing. + + Controls Distance Blurring. Lower Numbers decrease focus range, larger numbers increase it. + """ + + horizontal_aperture: float = 20.955 + """Horizontal aperture (in cm). Defaults to 20.955 cm. + + Emulates sensor/film width on a camera. + + Note: + The default value is the horizontal aperture of a 35 mm spherical projector. + """ + + vertical_aperture: float | None = None + r"""Vertical aperture (in mm). Defaults to None. + + Emulates sensor/film height on a camera. If None, then the vertical aperture is calculated based on the + horizontal aperture and the aspect ratio of the image to maintain squared pixels. This is calculated as: + + .. math:: + \text{vertical aperture} = \text{horizontal aperture} \times \frac{\text{height}}{\text{width}} + """ + + horizontal_aperture_offset: float = 0.0 + """Offsets Resolution/Film gate horizontally. Defaults to 0.0.""" + + vertical_aperture_offset: float = 0.0 + """Offsets Resolution/Film gate vertically. Defaults to 0.0.""" + + lock_camera: bool = True + """Locks the camera in the Omniverse viewport. Defaults to True. + + If True, then the camera remains fixed at its configured transform. This is useful when wanting to view + the camera output on the GUI and not accidentally moving the camera through the GUI interactions. + """ + + @classmethod + def from_intrinsic_matrix( + cls, + intrinsic_matrix: list[float], + width: int, + height: int, + clipping_range: tuple[float, float] = (0.01, 1e6), + focal_length: float | None = None, + focus_distance: float = 400.0, + f_stop: float = 0.0, + projection_type: str = "pinhole", + lock_camera: bool = True, + ) -> PinholeCameraCfg: + r"""Create a :class:`PinholeCameraCfg` class instance from an intrinsic matrix. + + The intrinsic matrix is a 3x3 matrix that defines the mapping between the 3D world coordinates and + the 2D image. The matrix is defined as: + + .. math:: + I_{cam} = \begin{bmatrix} + f_x & 0 & c_x \\ + 0 & f_y & c_y \\ + 0 & 0 & 1 + \\end{bmatrix}, + + where :math:`f_x` and :math:`f_y` are the focal length along x and y direction, while :math:`c_x` and :math:`c_y` are the + principle point offsets along x and y direction respectively. + + Args: + intrinsic_matrix: Intrinsic matrix of the camera in row-major format. + The matrix is defined as [f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1]. Shape is (9,). + width: Width of the image (in pixels). + height: Height of the image (in pixels). + clipping_range: Near and far clipping distances (in m). Defaults to (0.01, 1e6). + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None + focal_length will be calculated 1 / width. + focus_distance: Distance from the camera to the focus plane (in m). Defaults to 400.0 m. + f_stop: Lens aperture. Defaults to 0.0, which turns off focusing. + projection_type: Type of projection to use for the camera. Defaults to "pinhole". + lock_camera: Locks the camera in the Omniverse viewport. Defaults to True. + + Returns: + An instance of the :class:`PinholeCameraCfg` class. + """ + # raise not implemented error is projection type is not pinhole + if projection_type != "pinhole": + raise NotImplementedError("Only pinhole projection type is supported.") + + usd_camera_params = sensor_utils.convert_camera_intrinsics_to_usd( + intrinsic_matrix=intrinsic_matrix, height=height, width=width, focal_length=focal_length + ) + + return cls( + projection_type=projection_type, + clipping_range=clipping_range, + focal_length=usd_camera_params["focal_length"], + focus_distance=focus_distance, + f_stop=f_stop, + horizontal_aperture=usd_camera_params["horizontal_aperture"], + vertical_aperture=usd_camera_params["vertical_aperture"], + horizontal_aperture_offset=usd_camera_params["horizontal_aperture_offset"], + vertical_aperture_offset=usd_camera_params["vertical_aperture_offset"], + lock_camera=lock_camera, + ) + + +@configclass +class FisheyeCameraCfg(PinholeCameraCfg): + """Configuration parameters for a USD camera prim with `fish-eye camera`_ settings. + + For more information on the parameters, please refer to the + `camera documentation `__. + + .. note:: + The default values are taken from the `Replicator camera `__ + function. + + .. _fish-eye camera: https://en.wikipedia.org/wiki/Fisheye_lens + """ + + func: Callable = sensors.spawn_camera + + projection_type: Literal[ + "fisheyePolynomial", + "fisheyeSpherical", + "fisheyeKannalaBrandtK3", + "fisheyeRadTanThinPrism", + "omniDirectionalStereo", + ] = "fisheyePolynomial" + r"""Type of projection to use for the camera. Defaults to "fisheyePolynomial". + + Available options: + + - ``"fisheyePolynomial"``: Fisheye camera model with :math:`360^{\circ}` spherical projection. + - ``"fisheyeSpherical"``: Fisheye camera model with :math:`360^{\circ}` full-frame projection. + - ``"fisheyeKannalaBrandtK3"``: Fisheye camera model using the Kannala-Brandt K3 distortion model. + - ``"fisheyeRadTanThinPrism"``: Fisheye camera model that combines radial and tangential distortions. + - ``"omniDirectionalStereo"``: Fisheye camera model supporting :math:`360^{\circ}` stereoscopic imaging. + """ + + fisheye_nominal_width: float = 1936.0 + """Nominal width of fisheye lens model (in pixels). Defaults to 1936.0.""" + + fisheye_nominal_height: float = 1216.0 + """Nominal height of fisheye lens model (in pixels). Defaults to 1216.0.""" + + fisheye_optical_centre_x: float = 970.94244 + """Horizontal optical centre position of fisheye lens model (in pixels). Defaults to 970.94244.""" + + fisheye_optical_centre_y: float = 600.37482 + """Vertical optical centre position of fisheye lens model (in pixels). Defaults to 600.37482.""" + + fisheye_max_fov: float = 200.0 + """Maximum field of view of fisheye lens model (in degrees). Defaults to 200.0 degrees.""" + + fisheye_polynomial_a: float = 0.0 + """First component of fisheye polynomial. Defaults to 0.0.""" + + fisheye_polynomial_b: float = 0.00245 + """Second component of fisheye polynomial. Defaults to 0.00245.""" + + fisheye_polynomial_c: float = 0.0 + """Third component of fisheye polynomial. Defaults to 0.0.""" + + fisheye_polynomial_d: float = 0.0 + """Fourth component of fisheye polynomial. Defaults to 0.0.""" + + fisheye_polynomial_e: float = 0.0 + """Fifth component of fisheye polynomial. Defaults to 0.0.""" + + fisheye_polynomial_f: float = 0.0 + """Sixth component of fisheye polynomial. Defaults to 0.0.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/__init__.py new file mode 100644 index 00000000000..a8eb4144077 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/__init__.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for spawning primitive shapes in the simulation. + +NVIDIA Omniverse provides various primitive shapes that can be used to create USDGeom prims. Based +on the configuration, the spawned prim can be: + +* a visual mesh (no physics) +* a static collider (no rigid body) +* a rigid body (with collision and rigid body properties). + +""" + +from .shapes import spawn_capsule, spawn_cone, spawn_cuboid, spawn_cylinder, spawn_sphere +from .shapes_cfg import CapsuleCfg, ConeCfg, CuboidCfg, CylinderCfg, ShapeCfg, SphereCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes.py new file mode 100644 index 00000000000..9452ad5502e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes.py @@ -0,0 +1,306 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import isaacsim.core.utils.prims as prim_utils +from pxr import Usd + +from isaaclab.sim import schemas +from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone + +if TYPE_CHECKING: + from . import shapes_cfg + + +@clone +def spawn_sphere( + prim_path: str, + cfg: shapes_cfg.SphereCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USDGeom-based sphere prim with the given attributes. + + For more information, see `USDGeomSphere `_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn sphere if it doesn't exist. + attributes = {"radius": cfg.radius} + _spawn_geom_from_prim_type(prim_path, cfg, "Sphere", attributes, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_cuboid( + prim_path: str, + cfg: shapes_cfg.CuboidCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USDGeom-based cuboid prim with the given attributes. + + For more information, see `USDGeomCube `_. + + Note: + Since USD only supports cubes, we set the size of the cube to the minimum of the given size and + scale the cube accordingly. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + If a prim already exists at the given path. + """ + # resolve the scale + size = min(cfg.size) + scale = [dim / size for dim in cfg.size] + # spawn cuboid if it doesn't exist. + attributes = {"size": size} + _spawn_geom_from_prim_type(prim_path, cfg, "Cube", attributes, translation, orientation, scale) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_cylinder( + prim_path: str, + cfg: shapes_cfg.CylinderCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USDGeom-based cylinder prim with the given attributes. + + For more information, see `USDGeomCylinder `_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn cylinder if it doesn't exist. + attributes = {"radius": cfg.radius, "height": cfg.height, "axis": cfg.axis.upper()} + _spawn_geom_from_prim_type(prim_path, cfg, "Cylinder", attributes, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_capsule( + prim_path: str, + cfg: shapes_cfg.CapsuleCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USDGeom-based capsule prim with the given attributes. + + For more information, see `USDGeomCapsule `_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn capsule if it doesn't exist. + attributes = {"radius": cfg.radius, "height": cfg.height, "axis": cfg.axis.upper()} + _spawn_geom_from_prim_type(prim_path, cfg, "Capsule", attributes, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_cone( + prim_path: str, + cfg: shapes_cfg.ConeCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Create a USDGeom-based cone prim with the given attributes. + + For more information, see `USDGeomCone `_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn cone if it doesn't exist. + attributes = {"radius": cfg.radius, "height": cfg.height, "axis": cfg.axis.upper()} + _spawn_geom_from_prim_type(prim_path, cfg, "Cone", attributes, translation, orientation) + # return the prim + return prim_utils.get_prim_at_path(prim_path) + + +""" +Helper functions. +""" + + +def _spawn_geom_from_prim_type( + prim_path: str, + cfg: shapes_cfg.ShapeCfg, + prim_type: str, + attributes: dict, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + scale: tuple[float, float, float] | None = None, +): + """Create a USDGeom-based prim with the given attributes. + + To make the asset instanceable, we must follow a certain structure dictated by how USD scene-graph + instancing and physics work. The rigid body component must be added to each instance and not the + referenced asset (i.e. the prototype prim itself). This is because the rigid body component defines + properties that are specific to each instance and cannot be shared under the referenced asset. For + more information, please check the `documentation `_. + + Due to the above, we follow the following structure: + + * ``{prim_path}`` - The root prim that is an Xform with the rigid body and mass APIs if configured. + * ``{prim_path}/geometry`` - The prim that contains the mesh and optionally the materials if configured. + If instancing is enabled, this prim will be an instanceable reference to the prototype prim. + + Args: + prim_path: The prim path to spawn the asset at. + cfg: The config containing the properties to apply. + prim_type: The type of prim to create. + attributes: The attributes to apply to the prim. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + scale: The scale to apply to the prim. Defaults to None, in which case this is set to identity. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # spawn geometry if it doesn't exist. + if not prim_utils.is_prim_path_valid(prim_path): + prim_utils.create_prim(prim_path, prim_type="Xform", translation=translation, orientation=orientation) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + # create all the paths we need for clarity + geom_prim_path = prim_path + "/geometry" + mesh_prim_path = geom_prim_path + "/mesh" + + # create the geometry prim + prim_utils.create_prim(mesh_prim_path, prim_type, scale=scale, attributes=attributes) + # apply collision properties + if cfg.collision_props is not None: + schemas.define_collision_properties(mesh_prim_path, cfg.collision_props) + # apply visual material + if cfg.visual_material is not None: + if not cfg.visual_material_path.startswith("/"): + material_path = f"{geom_prim_path}/{cfg.visual_material_path}" + else: + material_path = cfg.visual_material_path + # create material + cfg.visual_material.func(material_path, cfg.visual_material) + # apply material + bind_visual_material(mesh_prim_path, material_path) + # apply physics material + if cfg.physics_material is not None: + if not cfg.physics_material_path.startswith("/"): + material_path = f"{geom_prim_path}/{cfg.physics_material_path}" + else: + material_path = cfg.physics_material_path + # create material + cfg.physics_material.func(material_path, cfg.physics_material) + # apply material + bind_physics_material(mesh_prim_path, material_path) + + # note: we apply rigid properties in the end to later make the instanceable prim + # apply mass properties + if cfg.mass_props is not None: + schemas.define_mass_properties(prim_path, cfg.mass_props) + # apply rigid body properties + if cfg.rigid_props is not None: + schemas.define_rigid_body_properties(prim_path, cfg.rigid_props) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes_cfg.py new file mode 100644 index 00000000000..231d042bf55 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/shapes/shapes_cfg.py @@ -0,0 +1,127 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sim.spawners import materials +from isaaclab.sim.spawners.spawner_cfg import RigidObjectSpawnerCfg +from isaaclab.utils import configclass + +from . import shapes + + +@configclass +class ShapeCfg(RigidObjectSpawnerCfg): + """Configuration parameters for a USD Geometry or Geom prim.""" + + visual_material_path: str = "material" + """Path to the visual material to use for the prim. Defaults to "material". + + If the path is relative, then it will be relative to the prim's path. + This parameter is ignored if `visual_material` is not None. + """ + visual_material: materials.VisualMaterialCfg | None = None + """Visual material properties. + + Note: + If None, then no visual material will be added. + """ + + physics_material_path: str = "material" + """Path to the physics material to use for the prim. Defaults to "material". + + If the path is relative, then it will be relative to the prim's path. + This parameter is ignored if `physics_material` is not None. + """ + physics_material: materials.PhysicsMaterialCfg | None = None + """Physics material properties. + + Note: + If None, then no physics material will be added. + """ + + +@configclass +class SphereCfg(ShapeCfg): + """Configuration parameters for a sphere prim. + + See :meth:`spawn_sphere` for more information. + """ + + func: Callable = shapes.spawn_sphere + + radius: float = MISSING + """Radius of the sphere (in m).""" + + +@configclass +class CuboidCfg(ShapeCfg): + """Configuration parameters for a cuboid prim. + + See :meth:`spawn_cuboid` for more information. + """ + + func: Callable = shapes.spawn_cuboid + + size: tuple[float, float, float] = MISSING + """Size of the cuboid.""" + + +@configclass +class CylinderCfg(ShapeCfg): + """Configuration parameters for a cylinder prim. + + See :meth:`spawn_cylinder` for more information. + """ + + func: Callable = shapes.spawn_cylinder + + radius: float = MISSING + """Radius of the cylinder (in m).""" + height: float = MISSING + """Height of the cylinder (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the cylinder. Defaults to "Z".""" + + +@configclass +class CapsuleCfg(ShapeCfg): + """Configuration parameters for a capsule prim. + + See :meth:`spawn_capsule` for more information. + """ + + func: Callable = shapes.spawn_capsule + + radius: float = MISSING + """Radius of the capsule (in m).""" + height: float = MISSING + """Height of the capsule (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the capsule. Defaults to "Z".""" + + +@configclass +class ConeCfg(ShapeCfg): + """Configuration parameters for a cone prim. + + See :meth:`spawn_cone` for more information. + """ + + func: Callable = shapes.spawn_cone + + radius: float = MISSING + """Radius of the cone (in m).""" + height: float = MISSING + """Height of the v (in m).""" + axis: Literal["X", "Y", "Z"] = "Z" + """Axis of the cone. Defaults to "Z".""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/spawner_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/spawner_cfg.py new file mode 100644 index 00000000000..8dfeb331f9b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/spawner_cfg.py @@ -0,0 +1,123 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import MISSING + +from pxr import Usd + +from isaaclab.sim import schemas +from isaaclab.utils import configclass + + +@configclass +class SpawnerCfg: + """Configuration parameters for spawning an asset. + + Spawning an asset is done by calling the :attr:`func` function. The function takes in the + prim path to spawn the asset at, the configuration instance and transformation, and returns the + prim path of the spawned asset. + + The function is typically decorated with :func:`isaaclab.sim.spawner.utils.clone` decorator + that checks if input prim path is a regex expression and spawns the asset at all matching prims. + For this, the decorator uses the Cloner API from Isaac Sim and handles the :attr:`copy_from_source` + parameter. + """ + + func: Callable[..., Usd.Prim] = MISSING + """Function to use for spawning the asset. + + The function takes in the prim path (or expression) to spawn the asset at, the configuration instance + and transformation, and returns the source prim spawned. + """ + + visible: bool = True + """Whether the spawned asset should be visible. Defaults to True.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset. Defaults to None, + which means no semantic tags will be added. + + The semantic tags follow the `Replicator Semantic` tagging system. Each tag is a tuple of the + form ``(type, data)``, where ``type`` is the type of the tag and ``data`` is the semantic label + associated with the tag. For example, to annotate a spawned asset in the class avocado, the semantic + tag would be ``[("class", "avocado")]``. + + You can specify multiple semantic tags by passing in a list of tags. For example, to annotate a + spawned asset in the class avocado and the color green, the semantic tags would be + ``[("class", "avocado"), ("color", "green")]``. + + .. seealso:: + + For more information on the semantics filter, see the documentation for the `semantics schema editor`_. + + .. _semantics schema editor: https://docs.omniverse.nvidia.com/extensions/latest/ext_replicator/semantics_schema_editor.html#semantics-filtering + + """ + + copy_from_source: bool = True + """Whether to copy the asset from the source prim or inherit it. Defaults to True. + + This parameter is only used when cloning prims. If False, then the asset will be inherited from + the source prim, i.e. all USD changes to the source prim will be reflected in the cloned prims. + """ + + +@configclass +class RigidObjectSpawnerCfg(SpawnerCfg): + """Configuration parameters for spawning a rigid asset. + + Note: + By default, all properties are set to None. This means that no properties will be added or modified + to the prim outside of the properties available by default when spawning the prim. + """ + + mass_props: schemas.MassPropertiesCfg | None = None + """Mass properties.""" + + rigid_props: schemas.RigidBodyPropertiesCfg | None = None + """Rigid body properties. + + For making a rigid object static, set the :attr:`schemas.RigidBodyPropertiesCfg.kinematic_enabled` + as True. This will make the object static and will not be affected by gravity or other forces. + """ + + collision_props: schemas.CollisionPropertiesCfg | None = None + """Properties to apply to all collision meshes.""" + + activate_contact_sensors: bool = False + """Activate contact reporting on all rigid bodies. Defaults to False. + + This adds the PhysxContactReporter API to all the rigid bodies in the given prim path and its children. + """ + + +@configclass +class DeformableObjectSpawnerCfg(SpawnerCfg): + """Configuration parameters for spawning a deformable asset. + + Unlike rigid objects, deformable objects are affected by forces and can deform when subjected to + external forces. This class is used to configure the properties of the deformable object. + + Deformable bodies don't have a separate collision mesh. The collision mesh is the same as the visual mesh. + The collision properties such as rest and collision offsets are specified in the :attr:`deformable_props`. + + Note: + By default, all properties are set to None. This means that no properties will be added or modified + to the prim outside of the properties available by default when spawning the prim. + """ + + mass_props: schemas.MassPropertiesCfg | None = None + """Mass properties.""" + + deformable_props: schemas.DeformableBodyPropertiesCfg | None = None + """Deformable body properties.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/__init__.py new file mode 100644 index 00000000000..0ee6f3b20b3 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for wrapping spawner configurations. + +Unlike the other spawner modules, this module provides a way to wrap multiple spawner configurations +into a single configuration. This is useful when the user wants to spawn multiple assets based on +different configurations. +""" + +from .wrappers import spawn_multi_asset, spawn_multi_usd_file +from .wrappers_cfg import MultiAssetSpawnerCfg, MultiUsdFileCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers.py new file mode 100644 index 00000000000..a3fa1502a7d --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers.py @@ -0,0 +1,182 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import random +import re +from typing import TYPE_CHECKING + +import carb +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils +from pxr import Sdf, Usd + +import isaaclab.sim as sim_utils +from isaaclab.sim.spawners.from_files import UsdFileCfg + +if TYPE_CHECKING: + from . import wrappers_cfg + + +def spawn_multi_asset( + prim_path: str, + cfg: wrappers_cfg.MultiAssetSpawnerCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn multiple assets based on the provided configurations. + + This function spawns multiple assets based on the provided configurations. The assets are spawned + in the order they are provided in the list. If the :attr:`~MultiAssetSpawnerCfg.random_choice` parameter is + set to True, a random asset configuration is selected for each spawn. + + Args: + prim_path: The prim path to spawn the assets. + cfg: The configuration for spawning the assets. + translation: The translation of the spawned assets. Default is None. + orientation: The orientation of the spawned assets in (w, x, y, z) order. Default is None. + + Returns: + The created prim at the first prim path. + """ + # resolve: {SPAWN_NS}/AssetName + # note: this assumes that the spawn namespace already exists in the stage + root_path, asset_path = prim_path.rsplit("/", 1) + # check if input is a regex expression + # note: a valid prim path can only contain alphanumeric characters, underscores, and forward slashes + is_regex_expression = re.match(r"^[a-zA-Z0-9/_]+$", root_path) is None + + # resolve matching prims for source prim path expression + if is_regex_expression and root_path != "": + source_prim_paths = sim_utils.find_matching_prim_paths(root_path) + # if no matching prims are found, raise an error + if len(source_prim_paths) == 0: + raise RuntimeError( + f"Unable to find source prim path: '{root_path}'. Please create the prim before spawning." + ) + else: + source_prim_paths = [root_path] + + # find a free prim path to hold all the template prims + template_prim_path = stage_utils.get_next_free_path("/World/Template") + prim_utils.create_prim(template_prim_path, "Scope") + + # spawn everything first in a "Dataset" prim + proto_prim_paths = list() + for index, asset_cfg in enumerate(cfg.assets_cfg): + # append semantic tags if specified + if cfg.semantic_tags is not None: + if asset_cfg.semantic_tags is None: + asset_cfg.semantic_tags = cfg.semantic_tags + else: + asset_cfg.semantic_tags += cfg.semantic_tags + # override settings for properties + attr_names = ["mass_props", "rigid_props", "collision_props", "activate_contact_sensors", "deformable_props"] + for attr_name in attr_names: + attr_value = getattr(cfg, attr_name) + if hasattr(asset_cfg, attr_name) and attr_value is not None: + setattr(asset_cfg, attr_name, attr_value) + # spawn single instance + proto_prim_path = f"{template_prim_path}/Asset_{index:04d}" + asset_cfg.func(proto_prim_path, asset_cfg, translation=translation, orientation=orientation) + # append to proto prim paths + proto_prim_paths.append(proto_prim_path) + + # resolve prim paths for spawning and cloning + prim_paths = [f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths] + + # acquire stage + stage = stage_utils.get_current_stage() + + # manually clone prims if the source prim path is a regex expression + # note: unlike in the cloner API from Isaac Sim, we do not "reset" xforms on the copied prims. + # This is because the "spawn" calls during the creation of the proto prims already handles this operation. + with Sdf.ChangeBlock(): + for index, prim_path in enumerate(prim_paths): + # spawn single instance + env_spec = Sdf.CreatePrimInLayer(stage.GetRootLayer(), prim_path) + # randomly select an asset configuration + if cfg.random_choice: + proto_path = random.choice(proto_prim_paths) + else: + proto_path = proto_prim_paths[index % len(proto_prim_paths)] + # copy the proto prim + Sdf.CopySpec(env_spec.layer, Sdf.Path(proto_path), env_spec.layer, Sdf.Path(prim_path)) + + # delete the dataset prim after spawning + prim_utils.delete_prim(template_prim_path) + + # set carb setting to indicate Isaac Lab's environments that different prims have been spawned + # at varying prim paths. In this case, PhysX parser shouldn't optimize the stage parsing. + # the flag is mainly used to inform the user that they should disable `InteractiveScene.replicate_physics` + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/isaaclab/spawn/multi_assets", True) + + # return the prim + return prim_utils.get_prim_at_path(prim_paths[0]) + + +def spawn_multi_usd_file( + prim_path: str, + cfg: wrappers_cfg.MultiUsdFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +) -> Usd.Prim: + """Spawn multiple USD files based on the provided configurations. + + This function creates configuration instances corresponding the individual USD files and + calls the :meth:`spawn_multi_asset` method to spawn them into the scene. + + Args: + prim_path: The prim path to spawn the assets. + cfg: The configuration for spawning the assets. + translation: The translation of the spawned assets. Default is None. + orientation: The orientation of the spawned assets in (w, x, y, z) order. Default is None. + + Returns: + The created prim at the first prim path. + """ + # needed here to avoid circular imports + from .wrappers_cfg import MultiAssetSpawnerCfg + + # parse all the usd files + if isinstance(cfg.usd_path, str): + usd_paths = [cfg.usd_path] + else: + usd_paths = cfg.usd_path + + # make a template usd config + usd_template_cfg = UsdFileCfg() + for attr_name, attr_value in cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func", "usd_path", "random_choice"]: + continue + # set the attribute into the template + setattr(usd_template_cfg, attr_name, attr_value) + + # create multi asset configuration of USD files + multi_asset_cfg = MultiAssetSpawnerCfg(assets_cfg=[]) + for usd_path in usd_paths: + usd_cfg = usd_template_cfg.replace(usd_path=usd_path) + multi_asset_cfg.assets_cfg.append(usd_cfg) + # set random choice + multi_asset_cfg.random_choice = cfg.random_choice + + # propagate the contact sensor settings + # note: the default value for activate_contact_sensors in MultiAssetSpawnerCfg is False. + # This ends up overwriting the usd-template-cfg's value when the `spawn_multi_asset` + # function is called. We hard-code the value to the usd-template-cfg's value to ensure + # that the contact sensor settings are propagated correctly. + if hasattr(cfg, "activate_contact_sensors"): + multi_asset_cfg.activate_contact_sensors = cfg.activate_contact_sensors + + # call the original function + return spawn_multi_asset(prim_path, multi_asset_cfg, translation, orientation) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers_cfg.py new file mode 100644 index 00000000000..d1478576692 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/spawners/wrappers/wrappers_cfg.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.sim.spawners.from_files import UsdFileCfg +from isaaclab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg, SpawnerCfg +from isaaclab.utils import configclass + +from . import wrappers + + +@configclass +class MultiAssetSpawnerCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): + """Configuration parameters for loading multiple assets from their individual configurations. + + Specifying values for any properties at the configuration level will override the settings of + individual assets' configuration. For instance if the attribute + :attr:`MultiAssetSpawnerCfg.mass_props` is specified, its value will overwrite the values of the + mass properties in each configuration inside :attr:`assets_cfg` (wherever applicable). + This is done to simplify configuring similar properties globally. By default, all properties are set to None. + + The following is an exception to the above: + + * :attr:`visible`: This parameter is ignored. Its value for the individual assets is used. + * :attr:`semantic_tags`: If specified, it will be appended to each individual asset's semantic tags. + + """ + + func = wrappers.spawn_multi_asset + + assets_cfg: list[SpawnerCfg] = MISSING + """List of asset configurations to spawn.""" + + random_choice: bool = True + """Whether to randomly select an asset configuration. Default is True. + + If False, the asset configurations are spawned in the order they are provided in the list. + If True, a random asset configuration is selected for each spawn. + """ + + +@configclass +class MultiUsdFileCfg(UsdFileCfg): + """Configuration parameters for loading multiple USD files. + + Specifying values for any properties at the configuration level is applied to all the assets + imported from their USD files. + + .. tip:: + It is recommended that all the USD based assets follow a similar prim-hierarchy. + + """ + + func = wrappers.spawn_multi_usd_file + + usd_path: str | list[str] = MISSING + """Path or a list of paths to the USD files to spawn asset from.""" + + random_choice: bool = True + """Whether to randomly select an asset configuration. Default is True. + + If False, the asset configurations are spawned in the order they are provided in the list. + If True, a random asset configuration is selected for each spawn. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/utils.py new file mode 100644 index 00000000000..fd145ea14a9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/sim/utils.py @@ -0,0 +1,869 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with USD-related utilities.""" + +from __future__ import annotations + +import functools +import inspect +import re +from collections.abc import Callable +from typing import TYPE_CHECKING, Any + +import isaacsim.core.utils.stage as stage_utils +import omni.kit.commands +import omni.log +from isaacsim.core.cloner import Cloner +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade + +# from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated +try: + import Semantics +except ModuleNotFoundError: + from pxr import Semantics + +from isaaclab.utils.string import to_camel_case + +from . import schemas + +if TYPE_CHECKING: + from .spawners.spawner_cfg import SpawnerCfg + +""" +Attribute - Setters. +""" + + +def safe_set_attribute_on_usd_schema(schema_api: Usd.APISchemaBase, name: str, value: Any, camel_case: bool): + """Set the value of an attribute on its USD schema if it exists. + + A USD API schema serves as an interface or API for authoring and extracting a set of attributes. + They typically derive from the :class:`pxr.Usd.SchemaBase` class. This function checks if the + attribute exists on the schema and sets the value of the attribute if it exists. + + Args: + schema_api: The USD schema to set the attribute on. + name: The name of the attribute. + value: The value to set the attribute to. + camel_case: Whether to convert the attribute name to camel case. + + Raises: + TypeError: When the input attribute name does not exist on the provided schema API. + """ + # if value is None, do nothing + if value is None: + return + # convert attribute name to camel case + if camel_case: + attr_name = to_camel_case(name, to="CC") + else: + attr_name = name + # retrieve the attribute + # reference: https://openusd.org/dev/api/_usd__page__common_idioms.html#Usd_Create_Or_Get_Property + attr = getattr(schema_api, f"Create{attr_name}Attr", None) + # check if attribute exists + if attr is not None: + attr().Set(value) + else: + # think: do we ever need to create the attribute if it doesn't exist? + # currently, we are not doing this since the schemas are already created with some defaults. + omni.log.error(f"Attribute '{attr_name}' does not exist on prim '{schema_api.GetPath()}'.") + raise TypeError(f"Attribute '{attr_name}' does not exist on prim '{schema_api.GetPath()}'.") + + +def safe_set_attribute_on_usd_prim(prim: Usd.Prim, attr_name: str, value: Any, camel_case: bool): + """Set the value of a attribute on its USD prim. + + The function creates a new attribute if it does not exist on the prim. This is because in some cases (such + as with shaders), their attributes are not exposed as USD prim properties that can be altered. This function + allows us to set the value of the attributes in these cases. + + Args: + prim: The USD prim to set the attribute on. + attr_name: The name of the attribute. + value: The value to set the attribute to. + camel_case: Whether to convert the attribute name to camel case. + """ + # if value is None, do nothing + if value is None: + return + # convert attribute name to camel case + if camel_case: + attr_name = to_camel_case(attr_name, to="cC") + # resolve sdf type based on value + if isinstance(value, bool): + sdf_type = Sdf.ValueTypeNames.Bool + elif isinstance(value, int): + sdf_type = Sdf.ValueTypeNames.Int + elif isinstance(value, float): + sdf_type = Sdf.ValueTypeNames.Float + elif isinstance(value, (tuple, list)) and len(value) == 3 and any(isinstance(v, float) for v in value): + sdf_type = Sdf.ValueTypeNames.Float3 + elif isinstance(value, (tuple, list)) and len(value) == 2 and any(isinstance(v, float) for v in value): + sdf_type = Sdf.ValueTypeNames.Float2 + else: + raise NotImplementedError( + f"Cannot set attribute '{attr_name}' with value '{value}'. Please modify the code to support this type." + ) + # change property + omni.kit.commands.execute( + "ChangePropertyCommand", + prop_path=Sdf.Path(f"{prim.GetPath()}.{attr_name}"), + value=value, + prev=None, + type_to_create_if_not_exist=sdf_type, + usd_context_name=prim.GetStage(), + ) + + +""" +Decorators. +""" + + +def apply_nested(func: Callable) -> Callable: + """Decorator to apply a function to all prims under a specified prim-path. + + The function iterates over the provided prim path and all its children to apply input function + to all prims under the specified prim path. + + If the function succeeds to apply to a prim, it will not look at the children of that prim. + This is based on the physics behavior that nested schemas are not allowed. For example, a parent prim + and its child prim cannot both have a rigid-body schema applied on them, or it is not possible to + have nested articulations. + + While traversing the prims under the specified prim path, the function will throw a warning if it + does not succeed to apply the function to any prim. This is because the user may have intended to + apply the function to a prim that does not have valid attributes, or the prim may be an instanced prim. + + Args: + func: The function to apply to all prims under a specified prim-path. The function + must take the prim-path and other arguments. It should return a boolean indicating whether + the function succeeded or not. + + Returns: + The wrapped function that applies the function to all prims under a specified prim-path. + + Raises: + ValueError: If the prim-path does not exist on the stage. + """ + + @functools.wraps(func) + def wrapper(prim_path: str | Sdf.Path, *args, **kwargs): + # map args and kwargs to function signature so we can get the stage + # note: we do this to check if stage is given in arg or kwarg + sig = inspect.signature(func) + bound_args = sig.bind(prim_path, *args, **kwargs) + # get current stage + stage = bound_args.arguments.get("stage") + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim: Usd.Prim = stage.GetPrimAtPath(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # add iterable to check if property was applied on any of the prims + count_success = 0 + instanced_prim_paths = [] + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + child_prim_path = child_prim.GetPath().pathString # type: ignore + # check if prim is a prototype + if child_prim.IsInstance(): + instanced_prim_paths.append(child_prim_path) + continue + # set properties + success = func(child_prim_path, *args, **kwargs) + # if successful, do not look at children + # this is based on the physics behavior that nested schemas are not allowed + if not success: + all_prims += child_prim.GetChildren() + else: + count_success += 1 + # check if we were successful in applying the function to any prim + if count_success == 0: + omni.log.warn( + f"Could not perform '{func.__name__}' on any prims under: '{prim_path}'." + " This might be because of the following reasons:" + "\n\t(1) The desired attribute does not exist on any of the prims." + "\n\t(2) The desired attribute exists on an instanced prim." + f"\n\t\tDiscovered list of instanced prim paths: {instanced_prim_paths}" + ) + + return wrapper + + +def clone(func: Callable) -> Callable: + """Decorator for cloning a prim based on matching prim paths of the prim's parent. + + The decorator checks if the parent prim path matches any prim paths in the stage. If so, it clones the + spawned prim at each matching prim path. For example, if the input prim path is: ``/World/Table_[0-9]/Bottle``, + the decorator will clone the prim at each matching prim path of the parent prim: ``/World/Table_0/Bottle``, + ``/World/Table_1/Bottle``, etc. + + Note: + For matching prim paths, the decorator assumes that valid prims exist for all matching prim paths. + In case no matching prim paths are found, the decorator raises a ``RuntimeError``. + + Args: + func: The function to decorate. + + Returns: + The decorated function that spawns the prim and clones it at each matching prim path. + It returns the spawned source prim, i.e., the first prim in the list of matching prim paths. + """ + + @functools.wraps(func) + def wrapper(prim_path: str | Sdf.Path, cfg: SpawnerCfg, *args, **kwargs): + # cast prim_path to str type in case its an Sdf.Path + prim_path = str(prim_path) + # check prim path is global + if not prim_path.startswith("/"): + raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") + # resolve: {SPAWN_NS}/AssetName + # note: this assumes that the spawn namespace already exists in the stage + root_path, asset_path = prim_path.rsplit("/", 1) + # check if input is a regex expression + # note: a valid prim path can only contain alphanumeric characters, underscores, and forward slashes + is_regex_expression = re.match(r"^[a-zA-Z0-9/_]+$", root_path) is None + + # resolve matching prims for source prim path expression + if is_regex_expression and root_path != "": + source_prim_paths = find_matching_prim_paths(root_path) + # if no matching prims are found, raise an error + if len(source_prim_paths) == 0: + raise RuntimeError( + f"Unable to find source prim path: '{root_path}'. Please create the prim before spawning." + ) + else: + source_prim_paths = [root_path] + + # resolve prim paths for spawning and cloning + prim_paths = [f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths] + # spawn single instance + prim = func(prim_paths[0], cfg, *args, **kwargs) + # set the prim visibility + if hasattr(cfg, "visible"): + imageable = UsdGeom.Imageable(prim) + if cfg.visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + # set the semantic annotations + if hasattr(cfg, "semantic_tags") and cfg.semantic_tags is not None: + # note: taken from replicator scripts.utils.utils.py + for semantic_type, semantic_value in cfg.semantic_tags: + # deal with spaces by replacing them with underscores + semantic_type_sanitized = semantic_type.replace(" ", "_") + semantic_value_sanitized = semantic_value.replace(" ", "_") + # set the semantic API for the instance + instance_name = f"{semantic_type_sanitized}_{semantic_value_sanitized}" + sem = Semantics.SemanticsAPI.Apply(prim, instance_name) + # create semantic type and data attributes + sem.CreateSemanticTypeAttr() + sem.CreateSemanticDataAttr() + sem.GetSemanticTypeAttr().Set(semantic_type) + sem.GetSemanticDataAttr().Set(semantic_value) + # activate rigid body contact sensors + if hasattr(cfg, "activate_contact_sensors") and cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_paths[0], cfg.activate_contact_sensors) + # clone asset using cloner API + if len(prim_paths) > 1: + cloner = Cloner() + # clone the prim + cloner.clone(prim_paths[0], prim_paths[1:], replicate_physics=False, copy_from_source=cfg.copy_from_source) + # return the source prim + return prim + + return wrapper + + +""" +Material bindings. +""" + + +@apply_nested +def bind_visual_material( + prim_path: str | Sdf.Path, + material_path: str | Sdf.Path, + stage: Usd.Stage | None = None, + stronger_than_descendants: bool = True, +): + """Bind a visual material to a prim. + + This function is a wrapper around the USD command `BindMaterialCommand`_. + + .. note:: + The function is decorated with :meth:`apply_nested` to allow applying the function to a prim path + and all its descendants. + + .. _BindMaterialCommand: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.BindMaterialCommand.html + + Args: + prim_path: The prim path where to apply the material. + material_path: The prim path of the material to apply. + stage: The stage where the prim and material exist. + Defaults to None, in which case the current stage is used. + stronger_than_descendants: Whether the material should override the material of its descendants. + Defaults to True. + + Raises: + ValueError: If the provided prim paths do not exist on stage. + """ + # resolve stage + if stage is None: + stage = stage_utils.get_current_stage() + # check if prim and material exists + if not stage.GetPrimAtPath(prim_path).IsValid(): + raise ValueError(f"Target prim '{material_path}' does not exist.") + if not stage.GetPrimAtPath(material_path).IsValid(): + raise ValueError(f"Visual material '{material_path}' does not exist.") + + # resolve token for weaker than descendants + if stronger_than_descendants: + binding_strength = "strongerThanDescendants" + else: + binding_strength = "weakerThanDescendants" + # obtain material binding API + # note: we prefer using the command here as it is more robust than the USD API + success, _ = omni.kit.commands.execute( + "BindMaterialCommand", + prim_path=prim_path, + material_path=material_path, + strength=binding_strength, + stage=stage, + ) + # return success + return success + + +@apply_nested +def bind_physics_material( + prim_path: str | Sdf.Path, + material_path: str | Sdf.Path, + stage: Usd.Stage | None = None, + stronger_than_descendants: bool = True, +): + """Bind a physics material to a prim. + + `Physics material`_ can be applied only to a prim with physics-enabled on them. This includes having + collision APIs, or deformable body APIs, or being a particle system. In case the prim does not have + any of these APIs, the function will not apply the material and return False. + + .. note:: + The function is decorated with :meth:`apply_nested` to allow applying the function to a prim path + and all its descendants. + + .. _Physics material: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials + + Args: + prim_path: The prim path where to apply the material. + material_path: The prim path of the material to apply. + stage: The stage where the prim and material exist. + Defaults to None, in which case the current stage is used. + stronger_than_descendants: Whether the material should override the material of its descendants. + Defaults to True. + + Raises: + ValueError: If the provided prim paths do not exist on stage. + """ + # resolve stage + if stage is None: + stage = stage_utils.get_current_stage() + # check if prim and material exists + if not stage.GetPrimAtPath(prim_path).IsValid(): + raise ValueError(f"Target prim '{material_path}' does not exist.") + if not stage.GetPrimAtPath(material_path).IsValid(): + raise ValueError(f"Physics material '{material_path}' does not exist.") + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim has collision applied on it + has_physics_scene_api = prim.HasAPI(PhysxSchema.PhysxSceneAPI) + has_collider = prim.HasAPI(UsdPhysics.CollisionAPI) + has_deformable_body = prim.HasAPI(PhysxSchema.PhysxDeformableBodyAPI) + has_particle_system = prim.IsA(PhysxSchema.PhysxParticleSystem) + if not (has_physics_scene_api or has_collider or has_deformable_body or has_particle_system): + omni.log.verbose( + f"Cannot apply physics material '{material_path}' on prim '{prim_path}'. It is neither a" + " PhysX scene, collider, a deformable body, nor a particle system." + ) + return False + + # obtain material binding API + if prim.HasAPI(UsdShade.MaterialBindingAPI): + material_binding_api = UsdShade.MaterialBindingAPI(prim) + else: + material_binding_api = UsdShade.MaterialBindingAPI.Apply(prim) + # obtain the material prim + material = UsdShade.Material(stage.GetPrimAtPath(material_path)) + # resolve token for weaker than descendants + if stronger_than_descendants: + binding_strength = UsdShade.Tokens.strongerThanDescendants + else: + binding_strength = UsdShade.Tokens.weakerThanDescendants + # apply the material + material_binding_api.Bind(material, bindingStrength=binding_strength, materialPurpose="physics") # type: ignore + # return success + return True + + +""" +Exporting. +""" + + +def export_prim_to_file( + path: str | Sdf.Path, + source_prim_path: str | Sdf.Path, + target_prim_path: str | Sdf.Path | None = None, + stage: Usd.Stage | None = None, +): + """Exports a prim from a given stage to a USD file. + + The function creates a new layer at the provided path and copies the prim to the layer. + It sets the copied prim as the default prim in the target layer. Additionally, it updates + the stage up-axis and meters-per-unit to match the current stage. + + Args: + path: The filepath path to export the prim to. + source_prim_path: The prim path to export. + target_prim_path: The prim path to set as the default prim in the target layer. + Defaults to None, in which case the source prim path is used. + stage: The stage where the prim exists. Defaults to None, in which case the + current stage is used. + + Raises: + ValueError: If the prim paths are not global (i.e: do not start with '/'). + """ + # automatically casting to str in case args + # are path types + path = str(path) + source_prim_path = str(source_prim_path) + if target_prim_path is not None: + target_prim_path = str(target_prim_path) + + if not source_prim_path.startswith("/"): + raise ValueError(f"Source prim path '{source_prim_path}' is not global. It must start with '/'.") + if target_prim_path is not None and not target_prim_path.startswith("/"): + raise ValueError(f"Target prim path '{target_prim_path}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage: Usd.Stage = omni.usd.get_context().get_stage() + # get root layer + source_layer = stage.GetRootLayer() + + # only create a new layer if it doesn't exist already + target_layer = Sdf.Find(path) + if target_layer is None: + target_layer = Sdf.Layer.CreateNew(path) + # open the target stage + target_stage = Usd.Stage.Open(target_layer) + + # update stage data + UsdGeom.SetStageUpAxis(target_stage, UsdGeom.GetStageUpAxis(stage)) + UsdGeom.SetStageMetersPerUnit(target_stage, UsdGeom.GetStageMetersPerUnit(stage)) + + # specify the prim to copy + source_prim_path = Sdf.Path(source_prim_path) + if target_prim_path is None: + target_prim_path = source_prim_path + + # copy the prim + Sdf.CreatePrimInLayer(target_layer, target_prim_path) + Sdf.CopySpec(source_layer, source_prim_path, target_layer, target_prim_path) + # set the default prim + target_layer.defaultPrim = Sdf.Path(target_prim_path).name + # resolve all paths relative to layer path + omni.usd.resolve_paths(source_layer.identifier, target_layer.identifier) + # save the stage + target_layer.Save() + + +""" +USD Prim properties. +""" + + +def make_uninstanceable(prim_path: str | Sdf.Path, stage: Usd.Stage | None = None): + """Check if a prim and its descendants are instanced and make them uninstanceable. + + This function checks if the prim at the specified prim path and its descendants are instanced. + If so, it makes the respective prim uninstanceable by disabling instancing on the prim. + + This is useful when we want to modify the properties of a prim that is instanced. For example, if we + want to apply a different material on an instanced prim, we need to make the prim uninstanceable first. + + Args: + prim_path: The prim path to check. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # make paths str type if they aren't already + prim_path = str(prim_path) + # check if prim path is global + if not prim_path.startswith("/"): + raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + # get prim + prim: Usd.Prim = stage.GetPrimAtPath(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # check if prim is instanced + if child_prim.IsInstance(): + # make the prim uninstanceable + child_prim.SetInstanceable(False) + # add children to list + all_prims += child_prim.GetChildren() + + +""" +USD Stage traversal. +""" + + +def get_first_matching_child_prim( + prim_path: str | Sdf.Path, predicate: Callable[[Usd.Prim], bool], stage: Usd.Stage | None = None +) -> Usd.Prim | None: + """Recursively get the first USD Prim at the path string that passes the predicate function + + Args: + prim_path: The path of the prim in the stage. + predicate: The function to test the prims against. It takes a prim as input and returns a boolean. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + The first prim on the path that passes the predicate. If no prim passes the predicate, it returns None. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # make paths str type if they aren't already + prim_path = str(prim_path) + # check if prim path is global + if not prim_path.startswith("/"): + raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + # get prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + # check if prim passes predicate + if predicate(child_prim): + return child_prim + # add children to list + all_prims += child_prim.GetChildren() + return None + + +def get_all_matching_child_prims( + prim_path: str | Sdf.Path, + predicate: Callable[[Usd.Prim], bool] = lambda _: True, + depth: int | None = None, + stage: Usd.Stage | None = None, +) -> list[Usd.Prim]: + """Performs a search starting from the root and returns all the prims matching the predicate. + + Args: + prim_path: The root prim path to start the search from. + predicate: The predicate that checks if the prim matches the desired criteria. It takes a prim as input + and returns a boolean. Defaults to a function that always returns True. + depth: The maximum depth for traversal, should be bigger than zero if specified. + Defaults to None (i.e: traversal happens till the end of the tree). + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + A list containing all the prims matching the predicate. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # make paths str type if they aren't already + prim_path = str(prim_path) + # check if prim path is global + if not prim_path.startswith("/"): + raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + # get prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # check if depth is valid + if depth is not None and depth <= 0: + raise ValueError(f"Depth must be bigger than zero, got {depth}.") + + # iterate over all prims under prim-path + # list of tuples (prim, current_depth) + all_prims_queue = [(prim, 0)] + output_prims = [] + while len(all_prims_queue) > 0: + # get current prim + child_prim, current_depth = all_prims_queue.pop(0) + # check if prim passes predicate + if predicate(child_prim): + output_prims.append(child_prim) + # add children to list + if depth is None or current_depth < depth: + all_prims_queue += [(child, current_depth + 1) for child in child_prim.GetChildren()] + + return output_prims + + +def find_first_matching_prim(prim_path_regex: str, stage: Usd.Stage | None = None) -> Usd.Prim | None: + """Find the first matching prim in the stage based on input regex expression. + + Args: + prim_path_regex: The regex expression for prim path. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + The first prim that matches input expression. If no prim matches, returns None. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # check prim path is global + if not prim_path_regex.startswith("/"): + raise ValueError(f"Prim path '{prim_path_regex}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + # need to wrap the token patterns in '^' and '$' to prevent matching anywhere in the string + pattern = f"^{prim_path_regex}$" + compiled_pattern = re.compile(pattern) + # obtain matching prim (depth-first search) + for prim in stage.Traverse(): + # check if prim passes predicate + if compiled_pattern.match(prim.GetPath().pathString) is not None: + return prim + return None + + +def find_matching_prims(prim_path_regex: str, stage: Usd.Stage | None = None) -> list[Usd.Prim]: + """Find all the matching prims in the stage based on input regex expression. + + Args: + prim_path_regex: The regex expression for prim path. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + A list of prims that match input expression. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # check prim path is global + if not prim_path_regex.startswith("/"): + raise ValueError(f"Prim path '{prim_path_regex}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + # need to wrap the token patterns in '^' and '$' to prevent matching anywhere in the string + tokens = prim_path_regex.split("/")[1:] + tokens = [f"^{token}$" for token in tokens] + # iterate over all prims in stage (breath-first search) + all_prims = [stage.GetPseudoRoot()] + output_prims = [] + for index, token in enumerate(tokens): + token_compiled = re.compile(token) + for prim in all_prims: + for child in prim.GetAllChildren(): + if token_compiled.match(child.GetName()) is not None: + output_prims.append(child) + if index < len(tokens) - 1: + all_prims = output_prims + output_prims = [] + return output_prims + + +def find_matching_prim_paths(prim_path_regex: str, stage: Usd.Stage | None = None) -> list[str]: + """Find all the matching prim paths in the stage based on input regex expression. + + Args: + prim_path_regex: The regex expression for prim path. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + A list of prim paths that match input expression. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + """ + # obtain matching prims + output_prims = find_matching_prims(prim_path_regex, stage) + # convert prims to prim paths + output_prim_paths = [] + for prim in output_prims: + output_prim_paths.append(prim.GetPath().pathString) + return output_prim_paths + + +def find_global_fixed_joint_prim( + prim_path: str | Sdf.Path, check_enabled_only: bool = False, stage: Usd.Stage | None = None +) -> UsdPhysics.Joint | None: + """Find the fixed joint prim under the specified prim path that connects the target to the simulation world. + + A joint is a connection between two bodies. A fixed joint is a joint that does not allow relative motion + between the two bodies. When a fixed joint has only one target body, it is considered to attach the body + to the simulation world. + + This function finds the fixed joint prim that has only one target under the specified prim path. If no such + fixed joint prim exists, it returns None. + + Args: + prim_path: The prim path to search for the fixed joint prim. + check_enabled_only: Whether to consider only enabled fixed joints. Defaults to False. + If False, then all joints (enabled or disabled) are considered. + stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. + + Returns: + The fixed joint prim that has only one target. If no such fixed joint prim exists, it returns None. + + Raises: + ValueError: If the prim path is not global (i.e: does not start with '/'). + ValueError: If the prim path does not exist on the stage. + """ + # check prim path is global + if not prim_path.startswith("/"): + raise ValueError(f"Prim path '{prim_path}' is not global. It must start with '/'.") + # get current stage + if stage is None: + stage = stage_utils.get_current_stage() + + # check if prim exists + prim = stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + + fixed_joint_prim = None + # we check all joints under the root prim and classify the asset as fixed base if there exists + # a fixed joint that has only one target (i.e. the root link). + for prim in Usd.PrimRange(prim): + # note: ideally checking if it is FixedJoint would have been enough, but some assets use "Joint" as the + # schema name which makes it difficult to distinguish between the two. + joint_prim = UsdPhysics.Joint(prim) + if joint_prim: + # if check_enabled_only is True, we only consider enabled joints + if check_enabled_only and not joint_prim.GetJointEnabledAttr().Get(): + continue + # check body 0 and body 1 exist + body_0_exist = joint_prim.GetBody0Rel().GetTargets() != [] + body_1_exist = joint_prim.GetBody1Rel().GetTargets() != [] + # if either body 0 or body 1 does not exist, we have a fixed joint that connects to the world + if not (body_0_exist and body_1_exist): + fixed_joint_prim = joint_prim + break + + return fixed_joint_prim + + +""" +USD Variants. +""" + + +def select_usd_variants(prim_path: str, variants: object | dict[str, str], stage: Usd.Stage | None = None): + """Sets the variant selections from the specified variant sets on a USD prim. + + `USD Variants`_ are a very powerful tool in USD composition that allows prims to have different options on + a single asset. This can be done by modifying variations of the same prim parameters per variant option in a set. + This function acts as a script-based utility to set the variant selections for the specified variant sets on a + USD prim. + + The function takes a dictionary or a config class mapping variant set names to variant selections. For instance, + if we have a prim at ``"/World/Table"`` with two variant sets: "color" and "size", we can set the variant + selections as follows: + + .. code-block:: python + + select_usd_variants( + prim_path="/World/Table", + variants={ + "color": "red", + "size": "large", + }, + ) + + Alternatively, we can use a config class to define the variant selections: + + .. code-block:: python + + @configclass + class TableVariants: + color: Literal["blue", "red"] = "red" + size: Literal["small", "large"] = "large" + + select_usd_variants( + prim_path="/World/Table", + variants=TableVariants(), + ) + + Args: + prim_path: The path of the USD prim. + variants: A dictionary or config class mapping variant set names to variant selections. + stage: The USD stage. Defaults to None, in which case, the current stage is used. + + Raises: + ValueError: If the prim at the specified path is not valid. + + .. _USD Variants: https://graphics.pixar.com/usd/docs/USD-Glossary.html#USDGlossary-Variant + """ + # Resolve stage + if stage is None: + stage = stage_utils.get_current_stage() + # Obtain prim + prim = stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # Convert to dict if we have a configclass object. + if not isinstance(variants, dict): + variants = variants.to_dict() + + existing_variant_sets = prim.GetVariantSets() + for variant_set_name, variant_selection in variants.items(): + # Check if the variant set exists on the prim. + if not existing_variant_sets.HasVariantSet(variant_set_name): + omni.log.warn(f"Variant set '{variant_set_name}' does not exist on prim '{prim_path}'.") + continue + + variant_set = existing_variant_sets.GetVariantSet(variant_set_name) + # Only set the variant selection if it is different from the current selection. + if variant_set.GetVariantSelection() != variant_selection: + variant_set.SetVariantSelection(variant_selection) + omni.log.info( + f"Setting variant selection '{variant_selection}' for variant set '{variant_set_name}' on" + f" prim '{prim_path}'." + ) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/__init__.py new file mode 100644 index 00000000000..2b7facb669b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package with utilities for creating terrains procedurally. + +There are two main components in this package: + +* :class:`TerrainGenerator`: This class procedurally generates terrains based on the passed + sub-terrain configuration. It creates a ``trimesh`` mesh object and contains the origins of + each generated sub-terrain. +* :class:`TerrainImporter`: This class mainly deals with importing terrains from different + possible sources and adding them to the simulator as a prim object. + The following functions are available for importing terrains: + + * :meth:`TerrainImporter.import_ground_plane`: spawn a grid plane which is default in Isaac Sim. + * :meth:`TerrainImporter.import_mesh`: spawn a prim from a ``trimesh`` object. + * :meth:`TerrainImporter.import_usd`: spawn a prim as reference to input USD file. + +""" + +from .height_field import * # noqa: F401, F403 +from .terrain_generator import TerrainGenerator +from .terrain_generator_cfg import FlatPatchSamplingCfg, SubTerrainBaseCfg, TerrainGeneratorCfg +from .terrain_importer import TerrainImporter +from .terrain_importer_cfg import TerrainImporterCfg +from .trimesh import * # noqa: F401, F403 +from .utils import color_meshes_by_height, create_prim_from_mesh diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/__init__.py new file mode 100644 index 00000000000..9613f47b324 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pre-defined terrain configurations for the terrain generator.""" + +from .rough import * # noqa: F401 diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/rough.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/rough.py new file mode 100644 index 00000000000..bd136a0eab1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/config/rough.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for custom terrains.""" + +import isaaclab.terrains as terrain_gen + +from ..terrain_generator_cfg import TerrainGeneratorCfg + +ROUGH_TERRAINS_CFG = TerrainGeneratorCfg( + size=(8.0, 8.0), + border_width=20.0, + num_rows=10, + num_cols=20, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + use_cache=False, + sub_terrains={ + "pyramid_stairs": terrain_gen.MeshPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.23), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + ), + "pyramid_stairs_inv": terrain_gen.MeshInvertedPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.23), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + ), + "boxes": terrain_gen.MeshRandomGridTerrainCfg( + proportion=0.2, grid_width=0.45, grid_height_range=(0.05, 0.2), platform_width=2.0 + ), + "random_rough": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.2, noise_range=(0.02, 0.10), noise_step=0.02, border_width=0.25 + ), + "hf_pyramid_slope": terrain_gen.HfPyramidSlopedTerrainCfg( + proportion=0.1, slope_range=(0.0, 0.4), platform_width=2.0, border_width=0.25 + ), + "hf_pyramid_slope_inv": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( + proportion=0.1, slope_range=(0.0, 0.4), platform_width=2.0, border_width=0.25 + ), + }, +) +"""Rough terrains configuration.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/__init__.py new file mode 100644 index 00000000000..d8b516159da --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/__init__.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This sub-module provides utilities to create different terrains as height fields (HF). + +Height fields are a 2.5D terrain representation that is used in robotics to obtain the +height of the terrain at a given point. This is useful for controls and planning algorithms. + +Each terrain is represented as a 2D numpy array with discretized heights. The shape of the array +is (width, length), where width and length are the number of points along the x and y axis, +respectively. The height of the terrain at a given point is obtained by indexing the array with +the corresponding x and y coordinates. + +.. caution:: + + When working with height field terrains, it is important to remember that the terrain is generated + from a discretized 3D representation. This means that the height of the terrain at a given point + is only an approximation of the real height of the terrain at that point. The discretization + error is proportional to the size of the discretization cells. Therefore, it is important to + choose a discretization size that is small enough for the application. A larger discretization + size will result in a faster simulation, but the terrain will be less accurate. + +""" + +from .hf_terrains_cfg import ( + HfDiscreteObstaclesTerrainCfg, + HfInvertedPyramidSlopedTerrainCfg, + HfInvertedPyramidStairsTerrainCfg, + HfPyramidSlopedTerrainCfg, + HfPyramidStairsTerrainCfg, + HfRandomUniformTerrainCfg, + HfSteppingStonesTerrainCfg, + HfTerrainBaseCfg, + HfWaveTerrainCfg, +) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains.py new file mode 100644 index 00000000000..6c5ea3b8860 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains.py @@ -0,0 +1,441 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions to generate height fields for different terrains.""" + +from __future__ import annotations + +import numpy as np +import scipy.interpolate as interpolate +from typing import TYPE_CHECKING + +from .utils import height_field_to_mesh + +if TYPE_CHECKING: + from . import hf_terrains_cfg + + +@height_field_to_mesh +def random_uniform_terrain(difficulty: float, cfg: hf_terrains_cfg.HfRandomUniformTerrainCfg) -> np.ndarray: + """Generate a terrain with height sampled uniformly from a specified range. + + .. image:: ../../_static/terrains/height_field/random_uniform_terrain.jpg + :width: 40% + :align: center + + Note: + The :obj:`difficulty` parameter is ignored for this terrain. + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + + Raises: + ValueError: When the downsampled scale is smaller than the horizontal scale. + """ + # check parameters + # -- horizontal scale + if cfg.downsampled_scale is None: + cfg.downsampled_scale = cfg.horizontal_scale + elif cfg.downsampled_scale < cfg.horizontal_scale: + raise ValueError( + "Downsampled scale must be larger than or equal to the horizontal scale:" + f" {cfg.downsampled_scale} < {cfg.horizontal_scale}." + ) + + # switch parameters to discrete units + # -- horizontal scale + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + # -- downsampled scale + width_downsampled = int(cfg.size[0] / cfg.downsampled_scale) + length_downsampled = int(cfg.size[1] / cfg.downsampled_scale) + # -- height + height_min = int(cfg.noise_range[0] / cfg.vertical_scale) + height_max = int(cfg.noise_range[1] / cfg.vertical_scale) + height_step = int(cfg.noise_step / cfg.vertical_scale) + + # create range of heights possible + height_range = np.arange(height_min, height_max + height_step, height_step) + # sample heights randomly from the range along a grid + height_field_downsampled = np.random.choice(height_range, size=(width_downsampled, length_downsampled)) + # create interpolation function for the sampled heights + x = np.linspace(0, cfg.size[0] * cfg.horizontal_scale, width_downsampled) + y = np.linspace(0, cfg.size[1] * cfg.horizontal_scale, length_downsampled) + func = interpolate.RectBivariateSpline(x, y, height_field_downsampled) + + # interpolate the sampled heights to obtain the height field + x_upsampled = np.linspace(0, cfg.size[0] * cfg.horizontal_scale, width_pixels) + y_upsampled = np.linspace(0, cfg.size[1] * cfg.horizontal_scale, length_pixels) + z_upsampled = func(x_upsampled, y_upsampled) + # round off the interpolated heights to the nearest vertical step + return np.rint(z_upsampled).astype(np.int16) + + +@height_field_to_mesh +def pyramid_sloped_terrain(difficulty: float, cfg: hf_terrains_cfg.HfPyramidSlopedTerrainCfg) -> np.ndarray: + """Generate a terrain with a truncated pyramid structure. + + The terrain is a pyramid-shaped sloped surface with a slope of :obj:`slope` that trims into a flat platform + at the center. The slope is defined as the ratio of the height change along the x axis to the width along the + x axis. For example, a slope of 1.0 means that the height changes by 1 unit for every 1 unit of width. + + If the :obj:`cfg.inverted` flag is set to :obj:`True`, the terrain is inverted such that + the platform is at the bottom. + + .. image:: ../../_static/terrains/height_field/pyramid_sloped_terrain.jpg + :width: 40% + + .. image:: ../../_static/terrains/height_field/inverted_pyramid_sloped_terrain.jpg + :width: 40% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + """ + # resolve terrain configuration + if cfg.inverted: + slope = -cfg.slope_range[0] - difficulty * (cfg.slope_range[1] - cfg.slope_range[0]) + else: + slope = cfg.slope_range[0] + difficulty * (cfg.slope_range[1] - cfg.slope_range[0]) + + # switch parameters to discrete units + # -- horizontal scale + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + # -- height + # we want the height to be 1/2 of the width since the terrain is a pyramid + height_max = int(slope * cfg.size[0] / 2 / cfg.vertical_scale) + # -- center of the terrain + center_x = int(width_pixels / 2) + center_y = int(length_pixels / 2) + + # create a meshgrid of the terrain + x = np.arange(0, width_pixels) + y = np.arange(0, length_pixels) + xx, yy = np.meshgrid(x, y, sparse=True) + # offset the meshgrid to the center of the terrain + xx = (center_x - np.abs(center_x - xx)) / center_x + yy = (center_y - np.abs(center_y - yy)) / center_y + # reshape the meshgrid to be 2D + xx = xx.reshape(width_pixels, 1) + yy = yy.reshape(1, length_pixels) + # create a sloped surface + hf_raw = np.zeros((width_pixels, length_pixels)) + hf_raw = height_max * xx * yy + + # create a flat platform at the center of the terrain + platform_width = int(cfg.platform_width / cfg.horizontal_scale / 2) + # get the height of the platform at the corner of the platform + x_pf = width_pixels // 2 - platform_width + y_pf = length_pixels // 2 - platform_width + z_pf = hf_raw[x_pf, y_pf] + hf_raw = np.clip(hf_raw, min(0, z_pf), max(0, z_pf)) + + # round off the heights to the nearest vertical step + return np.rint(hf_raw).astype(np.int16) + + +@height_field_to_mesh +def pyramid_stairs_terrain(difficulty: float, cfg: hf_terrains_cfg.HfPyramidStairsTerrainCfg) -> np.ndarray: + """Generate a terrain with a pyramid stair pattern. + + The terrain is a pyramid stair pattern which trims to a flat platform at the center of the terrain. + + If the :obj:`cfg.inverted` flag is set to :obj:`True`, the terrain is inverted such that + the platform is at the bottom. + + .. image:: ../../_static/terrains/height_field/pyramid_stairs_terrain.jpg + :width: 40% + + .. image:: ../../_static/terrains/height_field/inverted_pyramid_stairs_terrain.jpg + :width: 40% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + """ + # resolve terrain configuration + step_height = cfg.step_height_range[0] + difficulty * (cfg.step_height_range[1] - cfg.step_height_range[0]) + if cfg.inverted: + step_height *= -1 + # switch parameters to discrete units + # -- terrain + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + # -- stairs + step_width = int(cfg.step_width / cfg.horizontal_scale) + step_height = int(step_height / cfg.vertical_scale) + # -- platform + platform_width = int(cfg.platform_width / cfg.horizontal_scale) + + # create a terrain with a flat platform at the center + hf_raw = np.zeros((width_pixels, length_pixels)) + # add the steps + current_step_height = 0 + start_x, start_y = 0, 0 + stop_x, stop_y = width_pixels, length_pixels + while (stop_x - start_x) > platform_width and (stop_y - start_y) > platform_width: + # increment position + # -- x + start_x += step_width + stop_x -= step_width + # -- y + start_y += step_width + stop_y -= step_width + # increment height + current_step_height += step_height + # add the step + hf_raw[start_x:stop_x, start_y:stop_y] = current_step_height + + # round off the heights to the nearest vertical step + return np.rint(hf_raw).astype(np.int16) + + +@height_field_to_mesh +def discrete_obstacles_terrain(difficulty: float, cfg: hf_terrains_cfg.HfDiscreteObstaclesTerrainCfg) -> np.ndarray: + """Generate a terrain with randomly generated obstacles as pillars with positive and negative heights. + + The terrain is a flat platform at the center of the terrain with randomly generated obstacles as pillars + with positive and negative height. The obstacles are randomly generated cuboids with a random width and + height. They are placed randomly on the terrain with a minimum distance of :obj:`cfg.platform_width` + from the center of the terrain. + + .. image:: ../../_static/terrains/height_field/discrete_obstacles_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + """ + # resolve terrain configuration + obs_height = cfg.obstacle_height_range[0] + difficulty * ( + cfg.obstacle_height_range[1] - cfg.obstacle_height_range[0] + ) + + # switch parameters to discrete units + # -- terrain + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + # -- obstacles + obs_height = int(obs_height / cfg.vertical_scale) + obs_width_min = int(cfg.obstacle_width_range[0] / cfg.horizontal_scale) + obs_width_max = int(cfg.obstacle_width_range[1] / cfg.horizontal_scale) + # -- center of the terrain + platform_width = int(cfg.platform_width / cfg.horizontal_scale) + + # create discrete ranges for the obstacles + # -- shape + obs_width_range = np.arange(obs_width_min, obs_width_max, 4) + obs_length_range = np.arange(obs_width_min, obs_width_max, 4) + # -- position + obs_x_range = np.arange(0, width_pixels, 4) + obs_y_range = np.arange(0, length_pixels, 4) + + # create a terrain with a flat platform at the center + hf_raw = np.zeros((width_pixels, length_pixels)) + # generate the obstacles + for _ in range(cfg.num_obstacles): + # sample size + if cfg.obstacle_height_mode == "choice": + height = np.random.choice([-obs_height, -obs_height // 2, obs_height // 2, obs_height]) + elif cfg.obstacle_height_mode == "fixed": + height = obs_height + else: + raise ValueError(f"Unknown obstacle height mode '{cfg.obstacle_height_mode}'. Must be 'choice' or 'fixed'.") + width = int(np.random.choice(obs_width_range)) + length = int(np.random.choice(obs_length_range)) + # sample position + x_start = int(np.random.choice(obs_x_range)) + y_start = int(np.random.choice(obs_y_range)) + # clip start position to the terrain + if x_start + width > width_pixels: + x_start = width_pixels - width + if y_start + length > length_pixels: + y_start = length_pixels - length + # add to terrain + hf_raw[x_start : x_start + width, y_start : y_start + length] = height + # clip the terrain to the platform + x1 = (width_pixels - platform_width) // 2 + x2 = (width_pixels + platform_width) // 2 + y1 = (length_pixels - platform_width) // 2 + y2 = (length_pixels + platform_width) // 2 + hf_raw[x1:x2, y1:y2] = 0 + # round off the heights to the nearest vertical step + return np.rint(hf_raw).astype(np.int16) + + +@height_field_to_mesh +def wave_terrain(difficulty: float, cfg: hf_terrains_cfg.HfWaveTerrainCfg) -> np.ndarray: + r"""Generate a terrain with a wave pattern. + + The terrain is a flat platform at the center of the terrain with a wave pattern. The wave pattern + is generated by adding sinusoidal waves based on the number of waves and the amplitude of the waves. + + The height of the terrain at a point :math:`(x, y)` is given by: + + .. math:: + + h(x, y) = A \left(\sin\left(\frac{2 \pi x}{\lambda}\right) + \cos\left(\frac{2 \pi y}{\lambda}\right) \right) + + where :math:`A` is the amplitude of the waves, :math:`\lambda` is the wavelength of the waves. + + .. image:: ../../_static/terrains/height_field/wave_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + + Raises: + ValueError: When the number of waves is non-positive. + """ + # check number of waves + if cfg.num_waves < 0: + raise ValueError(f"Number of waves must be a positive integer. Got: {cfg.num_waves}.") + + # resolve terrain configuration + amplitude = cfg.amplitude_range[0] + difficulty * (cfg.amplitude_range[1] - cfg.amplitude_range[0]) + # switch parameters to discrete units + # -- terrain + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + amplitude_pixels = int(0.5 * amplitude / cfg.vertical_scale) + + # compute the wave number: nu = 2 * pi / lambda + wave_length = length_pixels / cfg.num_waves + wave_number = 2 * np.pi / wave_length + # create meshgrid for the terrain + x = np.arange(0, width_pixels) + y = np.arange(0, length_pixels) + xx, yy = np.meshgrid(x, y, sparse=True) + xx = xx.reshape(width_pixels, 1) + yy = yy.reshape(1, length_pixels) + + # create a terrain with a flat platform at the center + hf_raw = np.zeros((width_pixels, length_pixels)) + # add the waves + hf_raw += amplitude_pixels * (np.cos(yy * wave_number) + np.sin(xx * wave_number)) + # round off the heights to the nearest vertical step + return np.rint(hf_raw).astype(np.int16) + + +@height_field_to_mesh +def stepping_stones_terrain(difficulty: float, cfg: hf_terrains_cfg.HfSteppingStonesTerrainCfg) -> np.ndarray: + """Generate a terrain with a stepping stones pattern. + + The terrain is a stepping stones pattern which trims to a flat platform at the center of the terrain. + + .. image:: ../../_static/terrains/height_field/stepping_stones_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + The height field of the terrain as a 2D numpy array with discretized heights. + The shape of the array is (width, length), where width and length are the number of points + along the x and y axis, respectively. + """ + # resolve terrain configuration + stone_width = cfg.stone_width_range[1] - difficulty * (cfg.stone_width_range[1] - cfg.stone_width_range[0]) + stone_distance = cfg.stone_distance_range[0] + difficulty * ( + cfg.stone_distance_range[1] - cfg.stone_distance_range[0] + ) + + # switch parameters to discrete units + # -- terrain + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + # -- stones + stone_distance = int(stone_distance / cfg.horizontal_scale) + stone_width = int(stone_width / cfg.horizontal_scale) + stone_height_max = int(cfg.stone_height_max / cfg.vertical_scale) + # -- holes + holes_depth = int(cfg.holes_depth / cfg.vertical_scale) + # -- platform + platform_width = int(cfg.platform_width / cfg.horizontal_scale) + # create range of heights + stone_height_range = np.arange(-stone_height_max - 1, stone_height_max, step=1) + + # create a terrain with a flat platform at the center + hf_raw = np.full((width_pixels, length_pixels), holes_depth) + # add the stones + start_x, start_y = 0, 0 + # -- if the terrain is longer than it is wide then fill the terrain column by column + if length_pixels >= width_pixels: + while start_y < length_pixels: + # ensure that stone stops along y-axis + stop_y = min(length_pixels, start_y + stone_width) + # randomly sample x-position + start_x = np.random.randint(0, stone_width) + stop_x = max(0, start_x - stone_distance) + # fill first stone + hf_raw[0:stop_x, start_y:stop_y] = np.random.choice(stone_height_range) + # fill row with stones + while start_x < width_pixels: + stop_x = min(width_pixels, start_x + stone_width) + hf_raw[start_x:stop_x, start_y:stop_y] = np.random.choice(stone_height_range) + start_x += stone_width + stone_distance + # update y-position + start_y += stone_width + stone_distance + elif width_pixels > length_pixels: + while start_x < width_pixels: + # ensure that stone stops along x-axis + stop_x = min(width_pixels, start_x + stone_width) + # randomly sample y-position + start_y = np.random.randint(0, stone_width) + stop_y = max(0, start_y - stone_distance) + # fill first stone + hf_raw[start_x:stop_x, 0:stop_y] = np.random.choice(stone_height_range) + # fill column with stones + while start_y < length_pixels: + stop_y = min(length_pixels, start_y + stone_width) + hf_raw[start_x:stop_x, start_y:stop_y] = np.random.choice(stone_height_range) + start_y += stone_width + stone_distance + # update x-position + start_x += stone_width + stone_distance + # add the platform in the center + x1 = (width_pixels - platform_width) // 2 + x2 = (width_pixels + platform_width) // 2 + y1 = (length_pixels - platform_width) // 2 + y2 = (length_pixels + platform_width) // 2 + hf_raw[x1:x2, y1:y2] = 0 + # round off the heights to the nearest vertical step + return np.rint(hf_raw).astype(np.int16) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains_cfg.py new file mode 100644 index 00000000000..9a98d52443e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/hf_terrains_cfg.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from ..terrain_generator_cfg import SubTerrainBaseCfg +from . import hf_terrains + + +@configclass +class HfTerrainBaseCfg(SubTerrainBaseCfg): + """The base configuration for height field terrains.""" + + border_width: float = 0.0 + """The width of the border/padding around the terrain (in m). Defaults to 0.0. + + The border width is subtracted from the :obj:`size` of the terrain. If non-zero, it must be + greater than or equal to the :obj:`horizontal scale`. + """ + horizontal_scale: float = 0.1 + """The discretization of the terrain along the x and y axes (in m). Defaults to 0.1.""" + vertical_scale: float = 0.005 + """The discretization of the terrain along the z axis (in m). Defaults to 0.005.""" + slope_threshold: float | None = None + """The slope threshold above which surfaces are made vertical. Defaults to None, + in which case no correction is applied.""" + + +""" +Different height field terrain configurations. +""" + + +@configclass +class HfRandomUniformTerrainCfg(HfTerrainBaseCfg): + """Configuration for a random uniform height field terrain.""" + + function = hf_terrains.random_uniform_terrain + + noise_range: tuple[float, float] = MISSING + """The minimum and maximum height noise (i.e. along z) of the terrain (in m).""" + noise_step: float = MISSING + """The minimum height (in m) change between two points.""" + downsampled_scale: float | None = None + """The distance between two randomly sampled points on the terrain. Defaults to None, + in which case the :obj:`horizontal scale` is used. + + The heights are sampled at this resolution and interpolation is performed for intermediate points. + This must be larger than or equal to the :obj:`horizontal scale`. + """ + + +@configclass +class HfPyramidSlopedTerrainCfg(HfTerrainBaseCfg): + """Configuration for a pyramid sloped height field terrain.""" + + function = hf_terrains.pyramid_sloped_terrain + + slope_range: tuple[float, float] = MISSING + """The slope of the terrain (in radians).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + inverted: bool = False + """Whether the pyramid is inverted. Defaults to False. + + If True, the terrain is inverted such that the platform is at the bottom and the slopes are upwards. + """ + + +@configclass +class HfInvertedPyramidSlopedTerrainCfg(HfPyramidSlopedTerrainCfg): + """Configuration for an inverted pyramid sloped height field terrain. + + Note: + This is a subclass of :class:`HfPyramidSlopedTerrainCfg` with :obj:`inverted` set to True. + We make it as a separate class to make it easier to distinguish between the two and match + the naming convention of the other terrains. + """ + + inverted: bool = True + + +@configclass +class HfPyramidStairsTerrainCfg(HfTerrainBaseCfg): + """Configuration for a pyramid stairs height field terrain.""" + + function = hf_terrains.pyramid_stairs_terrain + + step_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the steps (in m).""" + step_width: float = MISSING + """The width of the steps (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + inverted: bool = False + """Whether the pyramid stairs is inverted. Defaults to False. + + If True, the terrain is inverted such that the platform is at the bottom and the stairs are upwards. + """ + + +@configclass +class HfInvertedPyramidStairsTerrainCfg(HfPyramidStairsTerrainCfg): + """Configuration for an inverted pyramid stairs height field terrain. + + Note: + This is a subclass of :class:`HfPyramidStairsTerrainCfg` with :obj:`inverted` set to True. + We make it as a separate class to make it easier to distinguish between the two and match + the naming convention of the other terrains. + """ + + inverted: bool = True + + +@configclass +class HfDiscreteObstaclesTerrainCfg(HfTerrainBaseCfg): + """Configuration for a discrete obstacles height field terrain.""" + + function = hf_terrains.discrete_obstacles_terrain + + obstacle_height_mode: str = "choice" + """The mode to use for the obstacle height. Defaults to "choice". + + The following modes are supported: "choice", "fixed". + """ + obstacle_width_range: tuple[float, float] = MISSING + """The minimum and maximum width of the obstacles (in m).""" + obstacle_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the obstacles (in m).""" + num_obstacles: int = MISSING + """The number of obstacles to generate.""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class HfWaveTerrainCfg(HfTerrainBaseCfg): + """Configuration for a wave height field terrain.""" + + function = hf_terrains.wave_terrain + + amplitude_range: tuple[float, float] = MISSING + """The minimum and maximum amplitude of the wave (in m).""" + num_waves: int = 1.0 + """The number of waves to generate. Defaults to 1.0.""" + + +@configclass +class HfSteppingStonesTerrainCfg(HfTerrainBaseCfg): + """Configuration for a stepping stones height field terrain.""" + + function = hf_terrains.stepping_stones_terrain + + stone_height_max: float = MISSING + """The maximum height of the stones (in m).""" + stone_width_range: tuple[float, float] = MISSING + """The minimum and maximum width of the stones (in m).""" + stone_distance_range: tuple[float, float] = MISSING + """The minimum and maximum distance between stones (in m).""" + holes_depth: float = -10.0 + """The depth of the holes (negative obstacles). Defaults to -10.0.""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/utils.py new file mode 100644 index 00000000000..30324f00292 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/height_field/utils.py @@ -0,0 +1,178 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import copy +import functools +import numpy as np +import trimesh +from collections.abc import Callable +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from .hf_terrains_cfg import HfTerrainBaseCfg + + +def height_field_to_mesh(func: Callable) -> Callable: + """Decorator to convert a height field function to a mesh function. + + This decorator converts a height field function to a mesh function by sampling the heights + at a specified resolution and performing interpolation to obtain the intermediate heights. + Additionally, it adds a border around the terrain to avoid artifacts at the edges. + + Args: + func: The height field function to convert. The function should return a 2D numpy array + with the heights of the terrain. + + Returns: + The mesh function. The mesh function returns a tuple containing a list of ``trimesh`` + mesh objects and the origin of the terrain. + """ + + @functools.wraps(func) + def wrapper(difficulty: float, cfg: HfTerrainBaseCfg): + # check valid border width + if cfg.border_width > 0 and cfg.border_width < cfg.horizontal_scale: + raise ValueError( + f"The border width ({cfg.border_width}) must be greater than or equal to the" + f" horizontal scale ({cfg.horizontal_scale})." + ) + # allocate buffer for height field (with border) + width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + 1 + length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + 1 + border_pixels = int(cfg.border_width / cfg.horizontal_scale) + 1 + heights = np.zeros((width_pixels, length_pixels), dtype=np.int16) + # override size of the terrain to account for the border + sub_terrain_size = [width_pixels - 2 * border_pixels, length_pixels - 2 * border_pixels] + sub_terrain_size = [dim * cfg.horizontal_scale for dim in sub_terrain_size] + # update the config + terrain_size = copy.deepcopy(cfg.size) + cfg.size = tuple(sub_terrain_size) + # generate the height field + z_gen = func(difficulty, cfg) + # handle the border for the terrain + heights[border_pixels:-border_pixels, border_pixels:-border_pixels] = z_gen + # set terrain size back to config + cfg.size = terrain_size + + # convert to trimesh + vertices, triangles = convert_height_field_to_mesh( + heights, cfg.horizontal_scale, cfg.vertical_scale, cfg.slope_threshold + ) + mesh = trimesh.Trimesh(vertices=vertices, faces=triangles) + # compute origin + x1 = int((cfg.size[0] * 0.5 - 1) / cfg.horizontal_scale) + x2 = int((cfg.size[0] * 0.5 + 1) / cfg.horizontal_scale) + y1 = int((cfg.size[1] * 0.5 - 1) / cfg.horizontal_scale) + y2 = int((cfg.size[1] * 0.5 + 1) / cfg.horizontal_scale) + origin_z = np.max(heights[x1:x2, y1:y2]) * cfg.vertical_scale + origin = np.array([0.5 * cfg.size[0], 0.5 * cfg.size[1], origin_z]) + # return mesh and origin + return [mesh], origin + + return wrapper + + +def convert_height_field_to_mesh( + height_field: np.ndarray, horizontal_scale: float, vertical_scale: float, slope_threshold: float | None = None +) -> tuple[np.ndarray, np.ndarray]: + """Convert a height-field array to a triangle mesh represented by vertices and triangles. + + This function converts a height-field array to a triangle mesh represented by vertices and triangles. + The height-field array is assumed to be a 2D array of floats, where each element represents the height + of the terrain at that location. The height-field array is assumed to be in the form of a matrix, where + the first dimension represents the x-axis and the second dimension represents the y-axis. + + The function can also correct vertical surfaces above the provide slope threshold. This is helpful to + avoid having long vertical surfaces in the mesh. The correction is done by moving the vertices of the + vertical surfaces to minimum of the two neighboring vertices. + + The correction is done in the following way: + If :math:`\\frac{y_2 - y_1}{x_2 - x_1} > threshold`, then move A to A' (i.e., set :math:`x_1' = x_2`). + This is repeated along all directions. + + .. code-block:: none + + B(x_2,y_2) + /| + / | + / | + (x_1,y_1)A---A'(x_1',y_1) + + Args: + height_field: The input height-field array. + horizontal_scale: The discretization of the terrain along the x and y axis. + vertical_scale: The discretization of the terrain along the z axis. + slope_threshold: The slope threshold above which surfaces are made vertical. + Defaults to None, in which case no correction is applied. + + Returns: + The vertices and triangles of the mesh: + - **vertices** (np.ndarray(float)): Array of shape (num_vertices, 3). + Each row represents the location of each vertex (in m). + - **triangles** (np.ndarray(int)): Array of shape (num_triangles, 3). + Each row represents the indices of the 3 vertices connected by this triangle. + """ + # read height field + num_rows, num_cols = height_field.shape + # create a mesh grid of the height field + y = np.linspace(0, (num_cols - 1) * horizontal_scale, num_cols) + x = np.linspace(0, (num_rows - 1) * horizontal_scale, num_rows) + yy, xx = np.meshgrid(y, x) + # copy height field to avoid modifying the original array + hf = height_field.copy() + + # correct vertical surfaces above the slope threshold + if slope_threshold is not None: + # scale slope threshold based on the horizontal and vertical scale + slope_threshold *= horizontal_scale / vertical_scale + # allocate arrays to store the movement of the vertices + move_x = np.zeros((num_rows, num_cols)) + move_y = np.zeros((num_rows, num_cols)) + move_corners = np.zeros((num_rows, num_cols)) + # move vertices along the x-axis + move_x[: num_rows - 1, :] += hf[1:num_rows, :] - hf[: num_rows - 1, :] > slope_threshold + move_x[1:num_rows, :] -= hf[: num_rows - 1, :] - hf[1:num_rows, :] > slope_threshold + # move vertices along the y-axis + move_y[:, : num_cols - 1] += hf[:, 1:num_cols] - hf[:, : num_cols - 1] > slope_threshold + move_y[:, 1:num_cols] -= hf[:, : num_cols - 1] - hf[:, 1:num_cols] > slope_threshold + # move vertices along the corners + move_corners[: num_rows - 1, : num_cols - 1] += ( + hf[1:num_rows, 1:num_cols] - hf[: num_rows - 1, : num_cols - 1] > slope_threshold + ) + move_corners[1:num_rows, 1:num_cols] -= ( + hf[: num_rows - 1, : num_cols - 1] - hf[1:num_rows, 1:num_cols] > slope_threshold + ) + xx += (move_x + move_corners * (move_x == 0)) * horizontal_scale + yy += (move_y + move_corners * (move_y == 0)) * horizontal_scale + + # create vertices for the mesh + vertices = np.zeros((num_rows * num_cols, 3), dtype=np.float32) + vertices[:, 0] = xx.flatten() + vertices[:, 1] = yy.flatten() + vertices[:, 2] = hf.flatten() * vertical_scale + # create triangles for the mesh + triangles = -np.ones((2 * (num_rows - 1) * (num_cols - 1), 3), dtype=np.uint32) + for i in range(num_rows - 1): + ind0 = np.arange(0, num_cols - 1) + i * num_cols + ind1 = ind0 + 1 + ind2 = ind0 + num_cols + ind3 = ind2 + 1 + start = 2 * i * (num_cols - 1) + stop = start + 2 * (num_cols - 1) + triangles[start:stop:2, 0] = ind0 + triangles[start:stop:2, 1] = ind3 + triangles[start:stop:2, 2] = ind1 + triangles[start + 1 : stop : 2, 0] = ind0 + triangles[start + 1 : stop : 2, 1] = ind2 + triangles[start + 1 : stop : 2, 2] = ind3 + + return vertices, triangles diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator.py new file mode 100644 index 00000000000..765f1756ff6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator.py @@ -0,0 +1,392 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import os +import torch +import trimesh + +import omni.log + +from isaaclab.utils.dict import dict_to_md5_hash +from isaaclab.utils.io import dump_yaml +from isaaclab.utils.timer import Timer +from isaaclab.utils.warp import convert_to_warp_mesh + +from .height_field import HfTerrainBaseCfg +from .terrain_generator_cfg import FlatPatchSamplingCfg, SubTerrainBaseCfg, TerrainGeneratorCfg +from .trimesh.utils import make_border +from .utils import color_meshes_by_height, find_flat_patches + + +class TerrainGenerator: + r"""Terrain generator to handle different terrain generation functions. + + The terrains are represented as meshes. These are obtained either from height fields or by using the + `trimesh `__ library. The height field representation is more + flexible, but it is less computationally and memory efficient than the trimesh representation. + + All terrain generation functions take in the argument :obj:`difficulty` which determines the complexity + of the terrain. The difficulty is a number between 0 and 1, where 0 is the easiest and 1 is the hardest. + In most cases, the difficulty is used for linear interpolation between different terrain parameters. + For example, in a pyramid stairs terrain the step height is interpolated between the specified minimum + and maximum step height. + + Each sub-terrain has a corresponding configuration class that can be used to specify the parameters + of the terrain. The configuration classes are inherited from the :class:`SubTerrainBaseCfg` class + which contains the common parameters for all terrains. + + If a curriculum is used, the terrains are generated based on their difficulty parameter. + The difficulty is varied linearly over the number of rows (i.e. along x) with a small random value + added to the difficulty to ensure that the columns with the same sub-terrain type are not exactly + the same. The difficulty parameter for a sub-terrain at a given row is calculated as: + + .. math:: + + \text{difficulty} = \frac{\text{row_id} + \eta}{\text{num_rows}} \times (\text{upper} - \text{lower}) + \text{lower} + + where :math:`\eta\sim\mathcal{U}(0, 1)` is a random perturbation to the difficulty, and + :math:`(\text{lower}, \text{upper})` is the range of the difficulty parameter, specified using the + :attr:`~TerrainGeneratorCfg.difficulty_range` parameter. + + If a curriculum is not used, the terrains are generated randomly. In this case, the difficulty parameter + is randomly sampled from the specified range, given by the :attr:`~TerrainGeneratorCfg.difficulty_range` parameter: + + .. math:: + + \text{difficulty} \sim \mathcal{U}(\text{lower}, \text{upper}) + + If the :attr:`~TerrainGeneratorCfg.flat_patch_sampling` is specified for a sub-terrain, flat patches are sampled + on the terrain. These can be used for spawning robots, targets, etc. The sampled patches are stored + in the :obj:`flat_patches` dictionary. The key specifies the intention of the flat patches and the + value is a tensor containing the flat patches for each sub-terrain. + + If the flag :attr:`~TerrainGeneratorCfg.use_cache` is set to True, the terrains are cached based on their + sub-terrain configurations. This means that if the same sub-terrain configuration is used + multiple times, the terrain is only generated once and then reused. This is useful when + generating complex sub-terrains that take a long time to generate. + + .. attention:: + + The terrain generation has its own seed parameter. This is set using the :attr:`TerrainGeneratorCfg.seed` + parameter. If the seed is not set and the caching is disabled, the terrain generation may not be + completely reproducible. + + """ + + terrain_mesh: trimesh.Trimesh + """A single trimesh.Trimesh object for all the generated sub-terrains.""" + terrain_meshes: list[trimesh.Trimesh] + """List of trimesh.Trimesh objects for all the generated sub-terrains.""" + terrain_origins: np.ndarray + """The origin of each sub-terrain. Shape is (num_rows, num_cols, 3).""" + flat_patches: dict[str, torch.Tensor] + """A dictionary of sampled valid (flat) patches for each sub-terrain. + + The dictionary keys are the names of the flat patch sampling configurations. This maps to a + tensor containing the flat patches for each sub-terrain. The shape of the tensor is + (num_rows, num_cols, num_patches, 3). + + For instance, the key "root_spawn" maps to a tensor containing the flat patches for spawning an asset. + Similarly, the key "target_spawn" maps to a tensor containing the flat patches for setting targets. + """ + + def __init__(self, cfg: TerrainGeneratorCfg, device: str = "cpu"): + """Initialize the terrain generator. + + Args: + cfg: Configuration for the terrain generator. + device: The device to use for the flat patches tensor. + """ + # check inputs + if len(cfg.sub_terrains) == 0: + raise ValueError("No sub-terrains specified! Please add at least one sub-terrain.") + # store inputs + self.cfg = cfg + self.device = device + + # set common values to all sub-terrains config + for sub_cfg in self.cfg.sub_terrains.values(): + # size of all terrains + sub_cfg.size = self.cfg.size + # params for height field terrains + if isinstance(sub_cfg, HfTerrainBaseCfg): + sub_cfg.horizontal_scale = self.cfg.horizontal_scale + sub_cfg.vertical_scale = self.cfg.vertical_scale + sub_cfg.slope_threshold = self.cfg.slope_threshold + + # throw a warning if the cache is enabled but the seed is not set + if self.cfg.use_cache and self.cfg.seed is None: + omni.log.warn( + "Cache is enabled but the seed is not set. The terrain generation will not be reproducible." + " Please set the seed in the terrain generator configuration to make the generation reproducible." + ) + + # if the seed is not set, we assume there is a global seed set and use that. + # this ensures that the terrain is reproducible if the seed is set at the beginning of the program. + if self.cfg.seed is not None: + seed = self.cfg.seed + else: + seed = np.random.get_state()[1][0] + # set the seed for reproducibility + # note: we create a new random number generator to avoid affecting the global state + # in the other places where random numbers are used. + self.np_rng = np.random.default_rng(seed) + + # buffer for storing valid patches + self.flat_patches = {} + # create a list of all sub-terrains + self.terrain_meshes = list() + self.terrain_origins = np.zeros((self.cfg.num_rows, self.cfg.num_cols, 3)) + + # parse configuration and add sub-terrains + # create terrains based on curriculum or randomly + if self.cfg.curriculum: + with Timer("[INFO] Generating terrains based on curriculum took"): + self._generate_curriculum_terrains() + else: + with Timer("[INFO] Generating terrains randomly took"): + self._generate_random_terrains() + # add a border around the terrains + self._add_terrain_border() + # combine all the sub-terrains into a single mesh + self.terrain_mesh = trimesh.util.concatenate(self.terrain_meshes) + + # color the terrain mesh + if self.cfg.color_scheme == "height": + self.terrain_mesh = color_meshes_by_height(self.terrain_mesh) + elif self.cfg.color_scheme == "random": + self.terrain_mesh.visual.vertex_colors = self.np_rng.choice( + range(256), size=(len(self.terrain_mesh.vertices), 4) + ) + elif self.cfg.color_scheme == "none": + pass + else: + raise ValueError(f"Invalid color scheme: {self.cfg.color_scheme}.") + + # offset the entire terrain and origins so that it is centered + # -- terrain mesh + transform = np.eye(4) + transform[:2, -1] = -self.cfg.size[0] * self.cfg.num_rows * 0.5, -self.cfg.size[1] * self.cfg.num_cols * 0.5 + self.terrain_mesh.apply_transform(transform) + # -- terrain origins + self.terrain_origins += transform[:3, -1] + # -- valid patches + terrain_origins_torch = torch.tensor(self.terrain_origins, dtype=torch.float, device=self.device).unsqueeze(2) + for name, value in self.flat_patches.items(): + self.flat_patches[name] = value + terrain_origins_torch + + def __str__(self): + """Return a string representation of the terrain generator.""" + msg = "Terrain Generator:" + msg += f"\n\tSeed: {self.cfg.seed}" + msg += f"\n\tNumber of rows: {self.cfg.num_rows}" + msg += f"\n\tNumber of columns: {self.cfg.num_cols}" + msg += f"\n\tSub-terrain size: {self.cfg.size}" + msg += f"\n\tSub-terrain types: {list(self.cfg.sub_terrains.keys())}" + msg += f"\n\tCurriculum: {self.cfg.curriculum}" + msg += f"\n\tDifficulty range: {self.cfg.difficulty_range}" + msg += f"\n\tColor scheme: {self.cfg.color_scheme}" + msg += f"\n\tUse cache: {self.cfg.use_cache}" + if self.cfg.use_cache: + msg += f"\n\tCache directory: {self.cfg.cache_dir}" + + return msg + + """ + Terrain generator functions. + """ + + def _generate_random_terrains(self): + """Add terrains based on randomly sampled difficulty parameter.""" + # normalize the proportions of the sub-terrains + proportions = np.array([sub_cfg.proportion for sub_cfg in self.cfg.sub_terrains.values()]) + proportions /= np.sum(proportions) + # create a list of all terrain configs + sub_terrains_cfgs = list(self.cfg.sub_terrains.values()) + + # randomly sample sub-terrains + for index in range(self.cfg.num_rows * self.cfg.num_cols): + # coordinate index of the sub-terrain + (sub_row, sub_col) = np.unravel_index(index, (self.cfg.num_rows, self.cfg.num_cols)) + # randomly sample terrain index + sub_index = self.np_rng.choice(len(proportions), p=proportions) + # randomly sample difficulty parameter + difficulty = self.np_rng.uniform(*self.cfg.difficulty_range) + # generate terrain + mesh, origin = self._get_terrain_mesh(difficulty, sub_terrains_cfgs[sub_index]) + # add to sub-terrains + self._add_sub_terrain(mesh, origin, sub_row, sub_col, sub_terrains_cfgs[sub_index]) + + def _generate_curriculum_terrains(self): + """Add terrains based on the difficulty parameter.""" + # normalize the proportions of the sub-terrains + proportions = np.array([sub_cfg.proportion for sub_cfg in self.cfg.sub_terrains.values()]) + proportions /= np.sum(proportions) + + # find the sub-terrain index for each column + # we generate the terrains based on their proportion (not randomly sampled) + sub_indices = [] + for index in range(self.cfg.num_cols): + sub_index = np.min(np.where(index / self.cfg.num_cols + 0.001 < np.cumsum(proportions))[0]) + sub_indices.append(sub_index) + sub_indices = np.array(sub_indices, dtype=np.int32) + # create a list of all terrain configs + sub_terrains_cfgs = list(self.cfg.sub_terrains.values()) + + # curriculum-based sub-terrains + for sub_col in range(self.cfg.num_cols): + for sub_row in range(self.cfg.num_rows): + # vary the difficulty parameter linearly over the number of rows + # note: based on the proportion, multiple columns can have the same sub-terrain type. + # Thus to increase the diversity along the rows, we add a small random value to the difficulty. + # This ensures that the terrains are not exactly the same. For example, if the + # the row index is 2 and the number of rows is 10, the nominal difficulty is 0.2. + # We add a small random value to the difficulty to make it between 0.2 and 0.3. + lower, upper = self.cfg.difficulty_range + difficulty = (sub_row + self.np_rng.uniform()) / self.cfg.num_rows + difficulty = lower + (upper - lower) * difficulty + # generate terrain + mesh, origin = self._get_terrain_mesh(difficulty, sub_terrains_cfgs[sub_indices[sub_col]]) + # add to sub-terrains + self._add_sub_terrain(mesh, origin, sub_row, sub_col, sub_terrains_cfgs[sub_indices[sub_col]]) + + """ + Internal helper functions. + """ + + def _add_terrain_border(self): + """Add a surrounding border over all the sub-terrains into the terrain meshes.""" + # border parameters + border_size = ( + self.cfg.num_rows * self.cfg.size[0] + 2 * self.cfg.border_width, + self.cfg.num_cols * self.cfg.size[1] + 2 * self.cfg.border_width, + ) + inner_size = (self.cfg.num_rows * self.cfg.size[0], self.cfg.num_cols * self.cfg.size[1]) + border_center = ( + self.cfg.num_rows * self.cfg.size[0] / 2, + self.cfg.num_cols * self.cfg.size[1] / 2, + -self.cfg.border_height / 2, + ) + # border mesh + border_meshes = make_border(border_size, inner_size, height=abs(self.cfg.border_height), position=border_center) + border = trimesh.util.concatenate(border_meshes) + # update the faces to have minimal triangles + selector = ~(np.asarray(border.triangles)[:, :, 2] < -0.1).any(1) + border.update_faces(selector) + # add the border to the list of meshes + self.terrain_meshes.append(border) + + def _add_sub_terrain( + self, mesh: trimesh.Trimesh, origin: np.ndarray, row: int, col: int, sub_terrain_cfg: SubTerrainBaseCfg + ): + """Add input sub-terrain to the list of sub-terrains. + + This function adds the input sub-terrain mesh to the list of sub-terrains and updates the origin + of the sub-terrain in the list of origins. It also samples flat patches if specified. + + Args: + mesh: The mesh of the sub-terrain. + origin: The origin of the sub-terrain. + row: The row index of the sub-terrain. + col: The column index of the sub-terrain. + """ + # sample flat patches if specified + if sub_terrain_cfg.flat_patch_sampling is not None: + omni.log.info(f"Sampling flat patches for sub-terrain at (row, col): ({row}, {col})") + # convert the mesh to warp mesh + wp_mesh = convert_to_warp_mesh(mesh.vertices, mesh.faces, device=self.device) + # sample flat patches based on each patch configuration for that sub-terrain + for name, patch_cfg in sub_terrain_cfg.flat_patch_sampling.items(): + patch_cfg: FlatPatchSamplingCfg + # create the flat patches tensor (if not already created) + if name not in self.flat_patches: + self.flat_patches[name] = torch.zeros( + (self.cfg.num_rows, self.cfg.num_cols, patch_cfg.num_patches, 3), device=self.device + ) + # add the flat patches to the tensor + self.flat_patches[name][row, col] = find_flat_patches( + wp_mesh=wp_mesh, + origin=origin, + num_patches=patch_cfg.num_patches, + patch_radius=patch_cfg.patch_radius, + x_range=patch_cfg.x_range, + y_range=patch_cfg.y_range, + z_range=patch_cfg.z_range, + max_height_diff=patch_cfg.max_height_diff, + ) + + # transform the mesh to the correct position + transform = np.eye(4) + transform[0:2, -1] = (row + 0.5) * self.cfg.size[0], (col + 0.5) * self.cfg.size[1] + mesh.apply_transform(transform) + # add mesh to the list + self.terrain_meshes.append(mesh) + # add origin to the list + self.terrain_origins[row, col] = origin + transform[:3, -1] + + def _get_terrain_mesh(self, difficulty: float, cfg: SubTerrainBaseCfg) -> tuple[trimesh.Trimesh, np.ndarray]: + """Generate a sub-terrain mesh based on the input difficulty parameter. + + If caching is enabled, the sub-terrain is cached and loaded from the cache if it exists. + The cache is stored in the cache directory specified in the configuration. + + .. Note: + This function centers the 2D center of the mesh and its specified origin such that the + 2D center becomes :math:`(0, 0)` instead of :math:`(size[0] / 2, size[1] / 2). + + Args: + difficulty: The difficulty parameter. + cfg: The configuration of the sub-terrain. + + Returns: + The sub-terrain mesh and origin. + """ + # copy the configuration + cfg = cfg.copy() + # add other parameters to the sub-terrain configuration + cfg.difficulty = float(difficulty) + cfg.seed = self.cfg.seed + # generate hash for the sub-terrain + sub_terrain_hash = dict_to_md5_hash(cfg.to_dict()) + # generate the file name + sub_terrain_cache_dir = os.path.join(self.cfg.cache_dir, sub_terrain_hash) + sub_terrain_obj_filename = os.path.join(sub_terrain_cache_dir, "mesh.obj") + sub_terrain_csv_filename = os.path.join(sub_terrain_cache_dir, "origin.csv") + sub_terrain_meta_filename = os.path.join(sub_terrain_cache_dir, "cfg.yaml") + + # check if hash exists - if true, load the mesh and origin and return + if self.cfg.use_cache and os.path.exists(sub_terrain_obj_filename): + # load existing mesh + mesh = trimesh.load_mesh(sub_terrain_obj_filename, process=False) + origin = np.loadtxt(sub_terrain_csv_filename, delimiter=",") + # return the generated mesh + return mesh, origin + + # generate the terrain + meshes, origin = cfg.function(difficulty, cfg) + mesh = trimesh.util.concatenate(meshes) + # offset mesh such that they are in their center + transform = np.eye(4) + transform[0:2, -1] = -cfg.size[0] * 0.5, -cfg.size[1] * 0.5 + mesh.apply_transform(transform) + # change origin to be in the center of the sub-terrain + origin += transform[0:3, -1] + + # if caching is enabled, save the mesh and origin + if self.cfg.use_cache: + # create the cache directory + os.makedirs(sub_terrain_cache_dir, exist_ok=True) + # save the data + mesh.export(sub_terrain_obj_filename) + np.savetxt(sub_terrain_csv_filename, origin, delimiter=",", header="x,y,z") + dump_yaml(sub_terrain_meta_filename, cfg) + # return the generated mesh + return mesh, origin diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator_cfg.py new file mode 100644 index 00000000000..2ba5107c25b --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_generator_cfg.py @@ -0,0 +1,209 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Configuration classes defining the different terrains available. Each configuration class must +inherit from ``isaaclab.terrains.terrains_cfg.TerrainConfig`` and define the following attributes: + +- ``name``: Name of the terrain. This is used for the prim name in the USD stage. +- ``function``: Function to generate the terrain. This function must take as input the terrain difficulty + and the configuration parameters and return a `tuple with the `trimesh`` mesh object and terrain origin. +""" + +from __future__ import annotations + +import numpy as np +import trimesh +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + + +@configclass +class FlatPatchSamplingCfg: + """Configuration for sampling flat patches on the sub-terrain. + + For a given sub-terrain, this configuration specifies how to sample flat patches on the terrain. + The sampled flat patches can be used for spawning robots, targets, etc. + + Please check the function :meth:`~isaaclab.terrains.utils.find_flat_patches` for more details. + """ + + num_patches: int = MISSING + """Number of patches to sample.""" + + patch_radius: float | list[float] = MISSING + """Radius of the patches. + + A list of radii can be provided to check for patches of different sizes. This is useful to deal with + cases where the terrain may have holes or obstacles in some areas. + """ + + x_range: tuple[float, float] = (-1e6, 1e6) + """The range of x-coordinates to sample from. Defaults to (-1e6, 1e6). + + This range is internally clamped to the size of the terrain mesh. + """ + + y_range: tuple[float, float] = (-1e6, 1e6) + """The range of y-coordinates to sample from. Defaults to (-1e6, 1e6). + + This range is internally clamped to the size of the terrain mesh. + """ + + z_range: tuple[float, float] = (-1e6, 1e6) + """Allowed range of z-coordinates for the sampled patch. Defaults to (-1e6, 1e6).""" + + max_height_diff: float = MISSING + """Maximum allowed height difference between the highest and lowest points on the patch.""" + + +@configclass +class SubTerrainBaseCfg: + """Base class for terrain configurations. + + All the sub-terrain configurations must inherit from this class. + + The :attr:`size` attribute is the size of the generated sub-terrain. Based on this, the terrain must + extend from :math:`(0, 0)` to :math:`(size[0], size[1])`. + """ + + function: Callable[[float, SubTerrainBaseCfg], tuple[list[trimesh.Trimesh], np.ndarray]] = MISSING + """Function to generate the terrain. + + This function must take as input the terrain difficulty and the configuration parameters and + return a tuple with a list of ``trimesh`` mesh objects and the terrain origin. + """ + + proportion: float = 1.0 + """Proportion of the terrain to generate. Defaults to 1.0. + + This is used to generate a mix of terrains. The proportion corresponds to the probability of sampling + the particular terrain. For example, if there are two terrains, A and B, with proportions 0.3 and 0.7, + respectively, then the probability of sampling terrain A is 0.3 and the probability of sampling terrain B + is 0.7. + """ + + size: tuple[float, float] = (10.0, 10.0) + """The width (along x) and length (along y) of the terrain (in m). Defaults to (10.0, 10.0). + + In case the :class:`~isaaclab.terrains.TerrainImporterCfg` is used, this parameter gets overridden by + :attr:`isaaclab.scene.TerrainImporterCfg.size` attribute. + """ + + flat_patch_sampling: dict[str, FlatPatchSamplingCfg] | None = None + """Dictionary of configurations for sampling flat patches on the sub-terrain. Defaults to None, + in which case no flat patch sampling is performed. + + The keys correspond to the name of the flat patch sampling configuration and the values are the + corresponding configurations. + """ + + +@configclass +class TerrainGeneratorCfg: + """Configuration for the terrain generator.""" + + seed: int | None = None + """The seed for the random number generator. Defaults to None, in which case the seed from the + current NumPy's random state is used. + + When the seed is set, the random number generator is initialized with the given seed. This ensures + that the generated terrains are deterministic across different runs. If the seed is not set, the + seed from the current NumPy's random state is used. This assumes that the seed is set elsewhere in + the code. + """ + + curriculum: bool = False + """Whether to use the curriculum mode. Defaults to False. + + If True, the terrains are generated based on their difficulty parameter. Otherwise, + they are randomly generated. + """ + + size: tuple[float, float] = MISSING + """The width (along x) and length (along y) of each sub-terrain (in m). + + Note: + This value is passed on to all the sub-terrain configurations. + """ + + border_width: float = 0.0 + """The width of the border around the terrain (in m). Defaults to 0.0.""" + + border_height: float = 1.0 + """The height of the border around the terrain (in m). Defaults to 1.0. + + .. note:: + The default border extends below the ground. If you want to make the border above the ground, choose a negative value. + """ + + num_rows: int = 1 + """Number of rows of sub-terrains to generate. Defaults to 1.""" + + num_cols: int = 1 + """Number of columns of sub-terrains to generate. Defaults to 1.""" + + color_scheme: Literal["height", "random", "none"] = "none" + """Color scheme to use for the terrain. Defaults to "none". + + The available color schemes are: + + - "height": Color based on the height of the terrain. + - "random": Random color scheme. + - "none": No color scheme. + """ + + horizontal_scale: float = 0.1 + """The discretization of the terrain along the x and y axes (in m). Defaults to 0.1. + + This value is passed on to all the height field sub-terrain configurations. + """ + + vertical_scale: float = 0.005 + """The discretization of the terrain along the z axis (in m). Defaults to 0.005. + + This value is passed on to all the height field sub-terrain configurations. + """ + + slope_threshold: float | None = 0.75 + """The slope threshold above which surfaces are made vertical. Defaults to 0.75. + + If None no correction is applied. + + This value is passed on to all the height field sub-terrain configurations. + """ + + sub_terrains: dict[str, SubTerrainBaseCfg] = MISSING + """Dictionary of sub-terrain configurations. + + The keys correspond to the name of the sub-terrain configuration and the values are the corresponding + configurations. + """ + + difficulty_range: tuple[float, float] = (0.0, 1.0) + """The range of difficulty values for the sub-terrains. Defaults to (0.0, 1.0). + + If curriculum is enabled, the terrains will be generated based on this range in ascending order + of difficulty. Otherwise, the terrains will be generated based on this range in a random order. + """ + + use_cache: bool = False + """Whether to load the sub-terrain from cache if it exists. Defaults to False. + + If enabled, the generated terrains are stored in the cache directory. When generating terrains, the cache + is checked to see if the terrain already exists. If it does, the terrain is loaded from the cache. Otherwise, + the terrain is generated and stored in the cache. Caching can be used to speed up terrain generation. + """ + + cache_dir: str = "/tmp/isaaclab/terrains" + """The directory where the terrain cache is stored. Defaults to "/tmp/isaaclab/terrains".""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer.py new file mode 100644 index 00000000000..61a1adad924 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer.py @@ -0,0 +1,397 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import torch +import trimesh +from typing import TYPE_CHECKING + +import omni.log + +import isaaclab.sim as sim_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG + +from .terrain_generator import TerrainGenerator +from .utils import create_prim_from_mesh + +if TYPE_CHECKING: + from .terrain_importer_cfg import TerrainImporterCfg + + +class TerrainImporter: + r"""A class to handle terrain meshes and import them into the simulator. + + We assume that a terrain mesh comprises of sub-terrains that are arranged in a grid with + rows ``num_rows`` and columns ``num_cols``. The terrain origins are the positions of the sub-terrains + where the robot should be spawned. + + Based on the configuration, the terrain importer handles computing the environment origins from the sub-terrain + origins. In a typical setup, the number of sub-terrains (:math:`num\_rows \times num\_cols`) is smaller than + the number of environments (:math:`num\_envs`). In this case, the environment origins are computed by + sampling the sub-terrain origins. + + If a curriculum is used, it is possible to update the environment origins to terrain origins that correspond + to a harder difficulty. This is done by calling :func:`update_terrain_levels`. The idea comes from game-based + curriculum. For example, in a game, the player starts with easy levels and progresses to harder levels. + """ + + terrain_prim_paths: list[str] + """A list containing the USD prim paths to the imported terrains.""" + + terrain_origins: torch.Tensor | None + """The origins of the sub-terrains in the added terrain mesh. Shape is (num_rows, num_cols, 3). + + If terrain origins is not None, the environment origins are computed based on the terrain origins. + Otherwise, the environment origins are computed based on the grid spacing. + """ + + env_origins: torch.Tensor + """The origins of the environments. Shape is (num_envs, 3).""" + + def __init__(self, cfg: TerrainImporterCfg): + """Initialize the terrain importer. + + Args: + cfg: The configuration for the terrain importer. + + Raises: + ValueError: If input terrain type is not supported. + ValueError: If terrain type is 'generator' and no configuration provided for ``terrain_generator``. + ValueError: If terrain type is 'usd' and no configuration provided for ``usd_path``. + ValueError: If terrain type is 'usd' or 'plane' and no configuration provided for ``env_spacing``. + """ + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg + self.device = sim_utils.SimulationContext.instance().device # type: ignore + + # create buffers for the terrains + self.terrain_prim_paths = list() + self.terrain_origins = None + self.env_origins = None # assigned later when `configure_env_origins` is called + # private variables + self._terrain_flat_patches = dict() + + # auto-import the terrain based on the config + if self.cfg.terrain_type == "generator": + # check config is provided + if self.cfg.terrain_generator is None: + raise ValueError("Input terrain type is 'generator' but no value provided for 'terrain_generator'.") + # generate the terrain + terrain_generator = TerrainGenerator(cfg=self.cfg.terrain_generator, device=self.device) + self.import_mesh("terrain", terrain_generator.terrain_mesh) + # configure the terrain origins based on the terrain generator + self.configure_env_origins(terrain_generator.terrain_origins) + # refer to the flat patches + self._terrain_flat_patches = terrain_generator.flat_patches + elif self.cfg.terrain_type == "usd": + # check if config is provided + if self.cfg.usd_path is None: + raise ValueError("Input terrain type is 'usd' but no value provided for 'usd_path'.") + # import the terrain + self.import_usd("terrain", self.cfg.usd_path) + # configure the origins in a grid + self.configure_env_origins() + elif self.cfg.terrain_type == "plane": + # load the plane + self.import_ground_plane("terrain") + # configure the origins in a grid + self.configure_env_origins() + else: + raise ValueError(f"Terrain type '{self.cfg.terrain_type}' not available.") + + # set initial state of debug visualization + self.set_debug_vis(self.cfg.debug_vis) + + """ + Properties. + """ + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the terrain importer has a debug visualization implemented. + + This always returns True. + """ + return True + + @property + def flat_patches(self) -> dict[str, torch.Tensor]: + """A dictionary containing the sampled valid (flat) patches for the terrain. + + This is only available if the terrain type is 'generator'. For other terrain types, this feature + is not available and the function returns an empty dictionary. + + Please refer to the :attr:`TerrainGenerator.flat_patches` for more information. + """ + return self._terrain_flat_patches + + @property + def terrain_names(self) -> list[str]: + """A list of names of the imported terrains.""" + return [f"'{path.split('/')[-1]}'" for path in self.terrain_prim_paths] + + """ + Operations - Visibility. + """ + + def set_debug_vis(self, debug_vis: bool) -> bool: + """Set the debug visualization of the terrain importer. + + Args: + debug_vis: Whether to visualize the terrain origins. + + Returns: + Whether the debug visualization was successfully set. False if the terrain + importer does not support debug visualization. + + Raises: + RuntimeError: If terrain origins are not configured. + """ + # create a marker if necessary + if debug_vis: + if not hasattr(self, "origin_visualizer"): + self.origin_visualizer = VisualizationMarkers( + cfg=FRAME_MARKER_CFG.replace(prim_path="/Visuals/TerrainOrigin") + ) + if self.terrain_origins is not None: + self.origin_visualizer.visualize(self.terrain_origins.reshape(-1, 3)) + elif self.env_origins is not None: + self.origin_visualizer.visualize(self.env_origins.reshape(-1, 3)) + else: + raise RuntimeError("Terrain origins are not configured.") + # set visibility + self.origin_visualizer.set_visibility(True) + else: + if hasattr(self, "origin_visualizer"): + self.origin_visualizer.set_visibility(False) + # report success + return True + + """ + Operations - Import. + """ + + def import_ground_plane(self, name: str, size: tuple[float, float] = (2.0e6, 2.0e6)): + """Add a plane to the terrain importer. + + Args: + name: The name of the imported terrain. This name is used to create the USD prim + corresponding to the terrain. + size: The size of the plane. Defaults to (2.0e6, 2.0e6). + + Raises: + ValueError: If a terrain with the same name already exists. + """ + # create prim path for the terrain + prim_path = self.cfg.prim_path + f"/{name}" + # check if key exists + if prim_path in self.terrain_prim_paths: + raise ValueError( + f"A terrain with the name '{name}' already exists. Existing terrains: {', '.join(self.terrain_names)}." + ) + # store the mesh name + self.terrain_prim_paths.append(prim_path) + + # obtain ground plane color from the configured visual material + color = (0.0, 0.0, 0.0) + if self.cfg.visual_material is not None: + material = self.cfg.visual_material.to_dict() + # defaults to the `GroundPlaneCfg` color if diffuse color attribute is not found + if "diffuse_color" in material: + color = material["diffuse_color"] + else: + omni.log.warn( + "Visual material specified for ground plane but no diffuse color found." + " Using default color: (0.0, 0.0, 0.0)" + ) + + # get the mesh + ground_plane_cfg = sim_utils.GroundPlaneCfg(physics_material=self.cfg.physics_material, size=size, color=color) + ground_plane_cfg.func(prim_path, ground_plane_cfg) + + def import_mesh(self, name: str, mesh: trimesh.Trimesh): + """Import a mesh into the simulator. + + The mesh is imported into the simulator under the prim path ``cfg.prim_path/{key}``. The created path + contains the mesh as a :class:`pxr.UsdGeom` instance along with visual or physics material prims. + + Args: + name: The name of the imported terrain. This name is used to create the USD prim + corresponding to the terrain. + mesh: The mesh to import. + + Raises: + ValueError: If a terrain with the same name already exists. + """ + # create prim path for the terrain + prim_path = self.cfg.prim_path + f"/{name}" + # check if key exists + if prim_path in self.terrain_prim_paths: + raise ValueError( + f"A terrain with the name '{name}' already exists. Existing terrains: {', '.join(self.terrain_names)}." + ) + # store the mesh name + self.terrain_prim_paths.append(prim_path) + + # import the mesh + create_prim_from_mesh( + prim_path, mesh, visual_material=self.cfg.visual_material, physics_material=self.cfg.physics_material + ) + + def import_usd(self, name: str, usd_path: str): + """Import a mesh from a USD file. + + This function imports a USD file into the simulator as a terrain. It parses the USD file and + stores the mesh under the prim path ``cfg.prim_path/{key}``. If multiple meshes are present in + the USD file, only the first mesh is imported. + + The function doe not apply any material properties to the mesh. The material properties should + be defined in the USD file. + + Args: + name: The name of the imported terrain. This name is used to create the USD prim + corresponding to the terrain. + usd_path: The path to the USD file. + + Raises: + ValueError: If a terrain with the same name already exists. + """ + # create prim path for the terrain + prim_path = self.cfg.prim_path + f"/{name}" + # check if key exists + if prim_path in self.terrain_prim_paths: + raise ValueError( + f"A terrain with the name '{name}' already exists. Existing terrains: {', '.join(self.terrain_names)}." + ) + # store the mesh name + self.terrain_prim_paths.append(prim_path) + + # add the prim path + cfg = sim_utils.UsdFileCfg(usd_path=usd_path) + cfg.func(prim_path, cfg) + + """ + Operations - Origins. + """ + + def configure_env_origins(self, origins: np.ndarray | torch.Tensor | None = None): + """Configure the origins of the environments based on the added terrain. + + Args: + origins: The origins of the sub-terrains. Shape is (num_rows, num_cols, 3). + """ + # decide whether to compute origins in a grid or based on curriculum + if origins is not None: + # convert to numpy + if isinstance(origins, np.ndarray): + origins = torch.from_numpy(origins) + # store the origins + self.terrain_origins = origins.to(self.device, dtype=torch.float) + # compute environment origins + self.env_origins = self._compute_env_origins_curriculum(self.cfg.num_envs, self.terrain_origins) + else: + self.terrain_origins = None + # check if env spacing is valid + if self.cfg.env_spacing is None: + raise ValueError("Environment spacing must be specified for configuring grid-like origins.") + # compute environment origins + self.env_origins = self._compute_env_origins_grid(self.cfg.num_envs, self.cfg.env_spacing) + + def update_env_origins(self, env_ids: torch.Tensor, move_up: torch.Tensor, move_down: torch.Tensor): + """Update the environment origins based on the terrain levels.""" + # check if grid-like spawning + if self.terrain_origins is None: + return + # update terrain level for the envs + self.terrain_levels[env_ids] += 1 * move_up - 1 * move_down + # robots that solve the last level are sent to a random one + # the minimum level is zero + self.terrain_levels[env_ids] = torch.where( + self.terrain_levels[env_ids] >= self.max_terrain_level, + torch.randint_like(self.terrain_levels[env_ids], self.max_terrain_level), + torch.clip(self.terrain_levels[env_ids], 0), + ) + # update the env origins + self.env_origins[env_ids] = self.terrain_origins[self.terrain_levels[env_ids], self.terrain_types[env_ids]] + + """ + Internal helpers. + """ + + def _compute_env_origins_curriculum(self, num_envs: int, origins: torch.Tensor) -> torch.Tensor: + """Compute the origins of the environments defined by the sub-terrains origins.""" + # extract number of rows and cols + num_rows, num_cols = origins.shape[:2] + # maximum initial level possible for the terrains + if self.cfg.max_init_terrain_level is None: + max_init_level = num_rows - 1 + else: + max_init_level = min(self.cfg.max_init_terrain_level, num_rows - 1) + # store maximum terrain level possible + self.max_terrain_level = num_rows + # define all terrain levels and types available + self.terrain_levels = torch.randint(0, max_init_level + 1, (num_envs,), device=self.device) + self.terrain_types = torch.div( + torch.arange(num_envs, device=self.device), (num_envs / num_cols), rounding_mode="floor" + ).to(torch.long) + # create tensor based on number of environments + env_origins = torch.zeros(num_envs, 3, device=self.device) + env_origins[:] = origins[self.terrain_levels, self.terrain_types] + return env_origins + + def _compute_env_origins_grid(self, num_envs: int, env_spacing: float) -> torch.Tensor: + """Compute the origins of the environments in a grid based on configured spacing.""" + # create tensor based on number of environments + env_origins = torch.zeros(num_envs, 3, device=self.device) + # create a grid of origins + num_rows = np.ceil(num_envs / int(np.sqrt(num_envs))) + num_cols = np.ceil(num_envs / num_rows) + ii, jj = torch.meshgrid( + torch.arange(num_rows, device=self.device), torch.arange(num_cols, device=self.device), indexing="ij" + ) + env_origins[:, 0] = -(ii.flatten()[:num_envs] - (num_rows - 1) / 2) * env_spacing + env_origins[:, 1] = (jj.flatten()[:num_envs] - (num_cols - 1) / 2) * env_spacing + env_origins[:, 2] = 0.0 + return env_origins + + """ + Deprecated. + """ + + @property + def warp_meshes(self): + """A dictionary containing the terrain's names and their warp meshes. + + .. deprecated:: v2.1.0 + The `warp_meshes` attribute is deprecated. It is no longer stored inside the class. + """ + omni.log.warn( + "The `warp_meshes` attribute is deprecated. It is no longer stored inside the `TerrainImporter` class." + " Returning an empty dictionary." + ) + return {} + + @property + def meshes(self) -> dict[str, trimesh.Trimesh]: + """A dictionary containing the terrain's names and their tri-meshes. + + .. deprecated:: v2.1.0 + The `meshes` attribute is deprecated. It is no longer stored inside the class. + """ + omni.log.warn( + "The `meshes` attribute is deprecated. It is no longer stored inside the `TerrainImporter` class." + " Returning an empty dictionary." + ) + return {} diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer_cfg.py new file mode 100644 index 00000000000..7998788e5f8 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/terrain_importer_cfg.py @@ -0,0 +1,108 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING +from typing import TYPE_CHECKING, Literal + +import isaaclab.sim as sim_utils +from isaaclab.utils import configclass + +from .terrain_importer import TerrainImporter + +if TYPE_CHECKING: + from .terrain_generator_cfg import TerrainGeneratorCfg + + +@configclass +class TerrainImporterCfg: + """Configuration for the terrain manager.""" + + class_type: type = TerrainImporter + """The class to use for the terrain importer. + + Defaults to :class:`isaaclab.terrains.terrain_importer.TerrainImporter`. + """ + + collision_group: int = -1 + """The collision group of the terrain. Defaults to -1.""" + + prim_path: str = MISSING + """The absolute path of the USD terrain prim. + + All sub-terrains are imported relative to this prim path. + """ + + num_envs: int = 1 + """The number of environment origins to consider. Defaults to 1. + + In case, the :class:`~isaaclab.scene.InteractiveSceneCfg` is used, this parameter gets overridden by + :attr:`isaaclab.scene.InteractiveSceneCfg.num_envs` attribute. + """ + + terrain_type: Literal["generator", "plane", "usd"] = "generator" + """The type of terrain to generate. Defaults to "generator". + + Available options are "plane", "usd", and "generator". + """ + + terrain_generator: TerrainGeneratorCfg | None = None + """The terrain generator configuration. + + Only used if ``terrain_type`` is set to "generator". + """ + + usd_path: str | None = None + """The path to the USD file containing the terrain. + + Only used if ``terrain_type`` is set to "usd". + """ + + env_spacing: float | None = None + """The spacing between environment origins when defined in a grid. Defaults to None. + + Note: + This parameter is used only when the ``terrain_type`` is "plane" or "usd". + """ + + visual_material: sim_utils.VisualMaterialCfg | None = sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 0.0)) + """The visual material of the terrain. Defaults to a dark gray color material. + + This parameter is used for both the "generator" and "plane" terrains. + + - If the ``terrain_type`` is "generator", then the material is created at the path + ``{prim_path}/visualMaterial`` and applied to all the sub-terrains. + - If the ``terrain_type`` is "plane", then the diffuse color of the material is set to + to the grid color of the imported ground plane. + """ + + physics_material: sim_utils.RigidBodyMaterialCfg = sim_utils.RigidBodyMaterialCfg() + """The physics material of the terrain. Defaults to a default physics material. + + The material is created at the path: ``{prim_path}/physicsMaterial``. + + .. note:: + This parameter is used only when the ``terrain_type`` is "generator" or "plane". + """ + + max_init_terrain_level: int | None = None + """The maximum initial terrain level for defining environment origins. Defaults to None. + + The terrain levels are specified by the number of rows in the grid arrangement of + sub-terrains. If None, then the initial terrain level is set to the maximum + terrain level available (``num_rows - 1``). + + Note: + This parameter is used only when sub-terrain origins are defined. + """ + + debug_vis: bool = False + """Whether to enable visualization of terrain origins for the terrain. Defaults to False.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/__init__.py new file mode 100644 index 00000000000..763b53253f0 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This sub-module provides methods to create different terrains using the ``trimesh`` library. + +In contrast to the height-field representation, the trimesh representation does not +create arbitrarily small triangles. Instead, the terrain is represented as a single +tri-mesh primitive. Thus, this representation is more computationally and memory +efficient than the height-field representation, but it is not as flexible. +""" + +from .mesh_terrains_cfg import ( + MeshBoxTerrainCfg, + MeshFloatingRingTerrainCfg, + MeshGapTerrainCfg, + MeshInvertedPyramidStairsTerrainCfg, + MeshPitTerrainCfg, + MeshPlaneTerrainCfg, + MeshPyramidStairsTerrainCfg, + MeshRailsTerrainCfg, + MeshRandomGridTerrainCfg, + MeshRepeatedBoxesTerrainCfg, + MeshRepeatedCylindersTerrainCfg, + MeshRepeatedPyramidsTerrainCfg, + MeshStarTerrainCfg, +) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains.py new file mode 100644 index 00000000000..241069629aa --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains.py @@ -0,0 +1,862 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions to generate different terrains using the ``trimesh`` library.""" + +from __future__ import annotations + +import numpy as np +import scipy.spatial.transform as tf +import torch +import trimesh +from typing import TYPE_CHECKING + +from .utils import * # noqa: F401, F403 +from .utils import make_border, make_plane + +if TYPE_CHECKING: + from . import mesh_terrains_cfg + + +def flat_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshPlaneTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a flat terrain as a plane. + + .. image:: ../../_static/terrains/trimesh/flat_terrain.jpg + :width: 45% + :align: center + + Note: + The :obj:`difficulty` parameter is ignored for this terrain. + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # compute the position of the terrain + origin = (cfg.size[0] / 2.0, cfg.size[1] / 2.0, 0.0) + # compute the vertices of the terrain + plane_mesh = make_plane(cfg.size, 0.0, center_zero=False) + # return the tri-mesh and the position + return [plane_mesh], np.array(origin) + + +def pyramid_stairs_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshPyramidStairsTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a pyramid stair pattern. + + The terrain is a pyramid stair pattern which trims to a flat platform at the center of the terrain. + + If :obj:`cfg.holes` is True, the terrain will have pyramid stairs of length or width + :obj:`cfg.platform_width` (depending on the direction) with no steps in the remaining area. Additionally, + no border will be added. + + .. image:: ../../_static/terrains/trimesh/pyramid_stairs_terrain.jpg + :width: 45% + + .. image:: ../../_static/terrains/trimesh/pyramid_stairs_terrain_with_holes.jpg + :width: 45% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + step_height = cfg.step_height_range[0] + difficulty * (cfg.step_height_range[1] - cfg.step_height_range[0]) + + # compute number of steps in x and y direction + num_steps_x = (cfg.size[0] - 2 * cfg.border_width - cfg.platform_width) // (2 * cfg.step_width) + 1 + num_steps_y = (cfg.size[1] - 2 * cfg.border_width - cfg.platform_width) // (2 * cfg.step_width) + 1 + # we take the minimum number of steps in x and y direction + num_steps = int(min(num_steps_x, num_steps_y)) + + # initialize list of meshes + meshes_list = list() + + # generate the border if needed + if cfg.border_width > 0.0 and not cfg.holes: + # obtain a list of meshes for the border + border_center = [0.5 * cfg.size[0], 0.5 * cfg.size[1], -step_height / 2] + border_inner_size = (cfg.size[0] - 2 * cfg.border_width, cfg.size[1] - 2 * cfg.border_width) + make_borders = make_border(cfg.size, border_inner_size, step_height, border_center) + # add the border meshes to the list of meshes + meshes_list += make_borders + + # generate the terrain + # -- compute the position of the center of the terrain + terrain_center = [0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.0] + terrain_size = (cfg.size[0] - 2 * cfg.border_width, cfg.size[1] - 2 * cfg.border_width) + # -- generate the stair pattern + for k in range(num_steps): + # check if we need to add holes around the steps + if cfg.holes: + box_size = (cfg.platform_width, cfg.platform_width) + else: + box_size = (terrain_size[0] - 2 * k * cfg.step_width, terrain_size[1] - 2 * k * cfg.step_width) + # compute the quantities of the box + # -- location + box_z = terrain_center[2] + k * step_height / 2.0 + box_offset = (k + 0.5) * cfg.step_width + # -- dimensions + box_height = (k + 2) * step_height + # generate the boxes + # top/bottom + box_dims = (box_size[0], cfg.step_width, box_height) + # -- top + box_pos = (terrain_center[0], terrain_center[1] + terrain_size[1] / 2.0 - box_offset, box_z) + box_top = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- bottom + box_pos = (terrain_center[0], terrain_center[1] - terrain_size[1] / 2.0 + box_offset, box_z) + box_bottom = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # right/left + if cfg.holes: + box_dims = (cfg.step_width, box_size[1], box_height) + else: + box_dims = (cfg.step_width, box_size[1] - 2 * cfg.step_width, box_height) + # -- right + box_pos = (terrain_center[0] + terrain_size[0] / 2.0 - box_offset, terrain_center[1], box_z) + box_right = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- left + box_pos = (terrain_center[0] - terrain_size[0] / 2.0 + box_offset, terrain_center[1], box_z) + box_left = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # add the boxes to the list of meshes + meshes_list += [box_top, box_bottom, box_right, box_left] + + # generate final box for the middle of the terrain + box_dims = ( + terrain_size[0] - 2 * num_steps * cfg.step_width, + terrain_size[1] - 2 * num_steps * cfg.step_width, + (num_steps + 2) * step_height, + ) + box_pos = (terrain_center[0], terrain_center[1], terrain_center[2] + num_steps * step_height / 2) + box_middle = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + meshes_list.append(box_middle) + # origin of the terrain + origin = np.array([terrain_center[0], terrain_center[1], (num_steps + 1) * step_height]) + + return meshes_list, origin + + +def inverted_pyramid_stairs_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshInvertedPyramidStairsTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a inverted pyramid stair pattern. + + The terrain is an inverted pyramid stair pattern which trims to a flat platform at the center of the terrain. + + If :obj:`cfg.holes` is True, the terrain will have pyramid stairs of length or width + :obj:`cfg.platform_width` (depending on the direction) with no steps in the remaining area. Additionally, + no border will be added. + + .. image:: ../../_static/terrains/trimesh/inverted_pyramid_stairs_terrain.jpg + :width: 45% + + .. image:: ../../_static/terrains/trimesh/inverted_pyramid_stairs_terrain_with_holes.jpg + :width: 45% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + step_height = cfg.step_height_range[0] + difficulty * (cfg.step_height_range[1] - cfg.step_height_range[0]) + + # compute number of steps in x and y direction + num_steps_x = (cfg.size[0] - 2 * cfg.border_width - cfg.platform_width) // (2 * cfg.step_width) + 1 + num_steps_y = (cfg.size[1] - 2 * cfg.border_width - cfg.platform_width) // (2 * cfg.step_width) + 1 + # we take the minimum number of steps in x and y direction + num_steps = int(min(num_steps_x, num_steps_y)) + # total height of the terrain + total_height = (num_steps + 1) * step_height + + # initialize list of meshes + meshes_list = list() + + # generate the border if needed + if cfg.border_width > 0.0 and not cfg.holes: + # obtain a list of meshes for the border + border_center = [0.5 * cfg.size[0], 0.5 * cfg.size[1], -0.5 * step_height] + border_inner_size = (cfg.size[0] - 2 * cfg.border_width, cfg.size[1] - 2 * cfg.border_width) + make_borders = make_border(cfg.size, border_inner_size, step_height, border_center) + # add the border meshes to the list of meshes + meshes_list += make_borders + # generate the terrain + # -- compute the position of the center of the terrain + terrain_center = [0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.0] + terrain_size = (cfg.size[0] - 2 * cfg.border_width, cfg.size[1] - 2 * cfg.border_width) + # -- generate the stair pattern + for k in range(num_steps): + # check if we need to add holes around the steps + if cfg.holes: + box_size = (cfg.platform_width, cfg.platform_width) + else: + box_size = (terrain_size[0] - 2 * k * cfg.step_width, terrain_size[1] - 2 * k * cfg.step_width) + # compute the quantities of the box + # -- location + box_z = terrain_center[2] - total_height / 2 - (k + 1) * step_height / 2.0 + box_offset = (k + 0.5) * cfg.step_width + # -- dimensions + box_height = total_height - (k + 1) * step_height + # generate the boxes + # top/bottom + box_dims = (box_size[0], cfg.step_width, box_height) + # -- top + box_pos = (terrain_center[0], terrain_center[1] + terrain_size[1] / 2.0 - box_offset, box_z) + box_top = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- bottom + box_pos = (terrain_center[0], terrain_center[1] - terrain_size[1] / 2.0 + box_offset, box_z) + box_bottom = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # right/left + if cfg.holes: + box_dims = (cfg.step_width, box_size[1], box_height) + else: + box_dims = (cfg.step_width, box_size[1] - 2 * cfg.step_width, box_height) + # -- right + box_pos = (terrain_center[0] + terrain_size[0] / 2.0 - box_offset, terrain_center[1], box_z) + box_right = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- left + box_pos = (terrain_center[0] - terrain_size[0] / 2.0 + box_offset, terrain_center[1], box_z) + box_left = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # add the boxes to the list of meshes + meshes_list += [box_top, box_bottom, box_right, box_left] + # generate final box for the middle of the terrain + box_dims = ( + terrain_size[0] - 2 * num_steps * cfg.step_width, + terrain_size[1] - 2 * num_steps * cfg.step_width, + step_height, + ) + box_pos = (terrain_center[0], terrain_center[1], terrain_center[2] - total_height - step_height / 2) + box_middle = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + meshes_list.append(box_middle) + # origin of the terrain + origin = np.array([terrain_center[0], terrain_center[1], -(num_steps + 1) * step_height]) + + return meshes_list, origin + + +def random_grid_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshRandomGridTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with cells of random heights and fixed width. + + The terrain is generated in the x-y plane and has a height of 1.0. It is then divided into a grid of the + specified size :obj:`cfg.grid_width`. Each grid cell is then randomly shifted in the z-direction by a value uniformly + sampled between :obj:`cfg.grid_height_range`. At the center of the terrain, a platform of the specified width + :obj:`cfg.platform_width` is generated. + + If :obj:`cfg.holes` is True, the terrain will have randomized grid cells only along the plane extending + from the platform (like a plus sign). The remaining area remains empty and no border will be added. + + .. image:: ../../_static/terrains/trimesh/random_grid_terrain.jpg + :width: 45% + + .. image:: ../../_static/terrains/trimesh/random_grid_terrain_with_holes.jpg + :width: 45% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + + Raises: + ValueError: If the terrain is not square. This method only supports square terrains. + RuntimeError: If the grid width is large such that the border width is negative. + """ + # check to ensure square terrain + if cfg.size[0] != cfg.size[1]: + raise ValueError(f"The terrain must be square. Received size: {cfg.size}.") + # resolve the terrain configuration + grid_height = cfg.grid_height_range[0] + difficulty * (cfg.grid_height_range[1] - cfg.grid_height_range[0]) + + # initialize list of meshes + meshes_list = list() + # compute the number of boxes in each direction + num_boxes_x = int(cfg.size[0] / cfg.grid_width) + num_boxes_y = int(cfg.size[1] / cfg.grid_width) + # constant parameters + terrain_height = 1.0 + device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") + + # generate the border + border_width = cfg.size[0] - min(num_boxes_x, num_boxes_y) * cfg.grid_width + if border_width > 0: + # compute parameters for the border + border_center = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2) + border_inner_size = (cfg.size[0] - border_width, cfg.size[1] - border_width) + # create border meshes + make_borders = make_border(cfg.size, border_inner_size, terrain_height, border_center) + meshes_list += make_borders + else: + raise RuntimeError("Border width must be greater than 0! Adjust the parameter 'cfg.grid_width'.") + + # create a template grid of terrain height + grid_dim = [cfg.grid_width, cfg.grid_width, terrain_height] + grid_position = [0.5 * cfg.grid_width, 0.5 * cfg.grid_width, -terrain_height / 2] + template_box = trimesh.creation.box(grid_dim, trimesh.transformations.translation_matrix(grid_position)) + # extract vertices and faces of the box to create a template + template_vertices = template_box.vertices # (8, 3) + template_faces = template_box.faces + + # repeat the template box vertices to span the terrain (num_boxes_x * num_boxes_y, 8, 3) + vertices = torch.tensor(template_vertices, device=device).repeat(num_boxes_x * num_boxes_y, 1, 1) + # create a meshgrid to offset the vertices + x = torch.arange(0, num_boxes_x, device=device) + y = torch.arange(0, num_boxes_y, device=device) + xx, yy = torch.meshgrid(x, y, indexing="ij") + xx = xx.flatten().view(-1, 1) + yy = yy.flatten().view(-1, 1) + xx_yy = torch.cat((xx, yy), dim=1) + # offset the vertices + offsets = cfg.grid_width * xx_yy + border_width / 2 + vertices[:, :, :2] += offsets.unsqueeze(1) + # mask the vertices to create holes, s.t. only grids along the x and y axis are present + if cfg.holes: + # -- x-axis + mask_x = torch.logical_and( + (vertices[:, :, 0] > (cfg.size[0] - border_width - cfg.platform_width) / 2).all(dim=1), + (vertices[:, :, 0] < (cfg.size[0] + border_width + cfg.platform_width) / 2).all(dim=1), + ) + vertices_x = vertices[mask_x] + # -- y-axis + mask_y = torch.logical_and( + (vertices[:, :, 1] > (cfg.size[1] - border_width - cfg.platform_width) / 2).all(dim=1), + (vertices[:, :, 1] < (cfg.size[1] + border_width + cfg.platform_width) / 2).all(dim=1), + ) + vertices_y = vertices[mask_y] + # -- combine these vertices + vertices = torch.cat((vertices_x, vertices_y)) + # add noise to the vertices to have a random height over each grid cell + num_boxes = len(vertices) + # create noise for the z-axis + h_noise = torch.zeros((num_boxes, 3), device=device) + h_noise[:, 2].uniform_(-grid_height, grid_height) + # reshape noise to match the vertices (num_boxes, 4, 3) + # only the top vertices of the box are affected + vertices_noise = torch.zeros((num_boxes, 4, 3), device=device) + vertices_noise += h_noise.unsqueeze(1) + # add height only to the top vertices of the box + vertices[vertices[:, :, 2] == 0] += vertices_noise.view(-1, 3) + # move to numpy + vertices = vertices.reshape(-1, 3).cpu().numpy() + + # create faces for boxes (num_boxes, 12, 3). Each box has 6 faces, each face has 2 triangles. + faces = torch.tensor(template_faces, device=device).repeat(num_boxes, 1, 1) + face_offsets = torch.arange(0, num_boxes, device=device).unsqueeze(1).repeat(1, 12) * 8 + faces += face_offsets.unsqueeze(2) + # move to numpy + faces = faces.view(-1, 3).cpu().numpy() + # convert to trimesh + grid_mesh = trimesh.Trimesh(vertices=vertices, faces=faces) + meshes_list.append(grid_mesh) + + # add a platform in the center of the terrain that is accessible from all sides + dim = (cfg.platform_width, cfg.platform_width, terrain_height + grid_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2 + grid_height / 2) + box_platform = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(box_platform) + + # specify the origin of the terrain + origin = np.array([0.5 * cfg.size[0], 0.5 * cfg.size[1], grid_height]) + + return meshes_list, origin + + +def rails_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshRailsTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with box rails as extrusions. + + The terrain contains two sets of box rails created as extrusions. The first set (inner rails) is extruded from + the platform at the center of the terrain, and the second set is extruded between the first set of rails + and the terrain border. Each set of rails is extruded to the same height. + + .. image:: ../../_static/terrains/trimesh/rails_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. this is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + rail_height = cfg.rail_height_range[1] - difficulty * (cfg.rail_height_range[1] - cfg.rail_height_range[0]) + + # initialize list of meshes + meshes_list = list() + # extract quantities + rail_1_thickness, rail_2_thickness = cfg.rail_thickness_range + rail_center = (0.5 * cfg.size[0], 0.5 * cfg.size[1], rail_height * 0.5) + # constants for terrain generation + terrain_height = 1.0 + rail_2_ratio = 0.6 + + # generate first set of rails + rail_1_inner_size = (cfg.platform_width, cfg.platform_width) + rail_1_outer_size = (cfg.platform_width + 2.0 * rail_1_thickness, cfg.platform_width + 2.0 * rail_1_thickness) + meshes_list += make_border(rail_1_outer_size, rail_1_inner_size, rail_height, rail_center) + # generate second set of rails + rail_2_inner_x = cfg.platform_width + (cfg.size[0] - cfg.platform_width) * rail_2_ratio + rail_2_inner_y = cfg.platform_width + (cfg.size[1] - cfg.platform_width) * rail_2_ratio + rail_2_inner_size = (rail_2_inner_x, rail_2_inner_y) + rail_2_outer_size = (rail_2_inner_x + 2.0 * rail_2_thickness, rail_2_inner_y + 2.0 * rail_2_thickness) + meshes_list += make_border(rail_2_outer_size, rail_2_inner_size, rail_height, rail_center) + # generate the ground + dim = (cfg.size[0], cfg.size[1], terrain_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2) + ground_meshes = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(ground_meshes) + + # specify the origin of the terrain + origin = np.array([pos[0], pos[1], 0.0]) + + return meshes_list, origin + + +def pit_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshPitTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a pit with levels (stairs) leading out of the pit. + + The terrain contains a platform at the center and a staircase leading out of the pit. + The staircase is a series of steps that are aligned along the x- and y- axis. The steps are + created by extruding a ring along the x- and y- axis. If :obj:`is_double_pit` is True, the pit + contains two levels. + + .. image:: ../../_static/terrains/trimesh/pit_terrain.jpg + :width: 40% + + .. image:: ../../_static/terrains/trimesh/pit_terrain_with_two_levels.jpg + :width: 40% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + pit_depth = cfg.pit_depth_range[0] + difficulty * (cfg.pit_depth_range[1] - cfg.pit_depth_range[0]) + + # initialize list of meshes + meshes_list = list() + # extract quantities + inner_pit_size = (cfg.platform_width, cfg.platform_width) + total_depth = pit_depth + # constants for terrain generation + terrain_height = 1.0 + ring_2_ratio = 0.6 + + # if the pit is double, the inner ring is smaller to fit the second level + if cfg.double_pit: + # increase the total height of the pit + total_depth *= 2.0 + # reduce the size of the inner ring + inner_pit_x = cfg.platform_width + (cfg.size[0] - cfg.platform_width) * ring_2_ratio + inner_pit_y = cfg.platform_width + (cfg.size[1] - cfg.platform_width) * ring_2_ratio + inner_pit_size = (inner_pit_x, inner_pit_y) + + # generate the pit (outer ring) + pit_center = [0.5 * cfg.size[0], 0.5 * cfg.size[1], -total_depth * 0.5] + meshes_list += make_border(cfg.size, inner_pit_size, total_depth, pit_center) + # generate the second level of the pit (inner ring) + if cfg.double_pit: + pit_center[2] = -total_depth + meshes_list += make_border(inner_pit_size, (cfg.platform_width, cfg.platform_width), total_depth, pit_center) + # generate the ground + dim = (cfg.size[0], cfg.size[1], terrain_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -total_depth - terrain_height / 2) + ground_meshes = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(ground_meshes) + + # specify the origin of the terrain + origin = np.array([pos[0], pos[1], -total_depth]) + + return meshes_list, origin + + +def box_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshBoxTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with boxes (similar to a pyramid). + + The terrain has a ground with boxes on top of it that are stacked on top of each other. + The boxes are created by extruding a rectangle along the z-axis. If :obj:`double_box` is True, + then two boxes of height :obj:`box_height` are stacked on top of each other. + + .. image:: ../../_static/terrains/trimesh/box_terrain.jpg + :width: 40% + + .. image:: ../../_static/terrains/trimesh/box_terrain_with_two_boxes.jpg + :width: 40% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + box_height = cfg.box_height_range[0] + difficulty * (cfg.box_height_range[1] - cfg.box_height_range[0]) + + # initialize list of meshes + meshes_list = list() + # extract quantities + total_height = box_height + if cfg.double_box: + total_height *= 2.0 + # constants for terrain generation + terrain_height = 1.0 + box_2_ratio = 0.6 + + # Generate the top box + dim = (cfg.platform_width, cfg.platform_width, terrain_height + total_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], (total_height - terrain_height) / 2) + box_mesh = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(box_mesh) + # Generate the lower box + if cfg.double_box: + # calculate the size of the lower box + outer_box_x = cfg.platform_width + (cfg.size[0] - cfg.platform_width) * box_2_ratio + outer_box_y = cfg.platform_width + (cfg.size[1] - cfg.platform_width) * box_2_ratio + # create the lower box + dim = (outer_box_x, outer_box_y, terrain_height + total_height / 2) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], (total_height - terrain_height) / 2 - total_height / 4) + box_mesh = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(box_mesh) + # Generate the ground + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2) + dim = (cfg.size[0], cfg.size[1], terrain_height) + ground_mesh = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(ground_mesh) + + # specify the origin of the terrain + origin = np.array([pos[0], pos[1], total_height]) + + return meshes_list, origin + + +def gap_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshGapTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a gap around the platform. + + The terrain has a ground with a platform in the middle. The platform is surrounded by a gap + of width :obj:`gap_width` on all sides. + + .. image:: ../../_static/terrains/trimesh/gap_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + gap_width = cfg.gap_width_range[0] + difficulty * (cfg.gap_width_range[1] - cfg.gap_width_range[0]) + + # initialize list of meshes + meshes_list = list() + # constants for terrain generation + terrain_height = 1.0 + terrain_center = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2) + + # Generate the outer ring + inner_size = (cfg.platform_width + 2 * gap_width, cfg.platform_width + 2 * gap_width) + meshes_list += make_border(cfg.size, inner_size, terrain_height, terrain_center) + # Generate the inner box + box_dim = (cfg.platform_width, cfg.platform_width, terrain_height) + box = trimesh.creation.box(box_dim, trimesh.transformations.translation_matrix(terrain_center)) + meshes_list.append(box) + + # specify the origin of the terrain + origin = np.array([terrain_center[0], terrain_center[1], 0.0]) + + return meshes_list, origin + + +def floating_ring_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshFloatingRingTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a floating square ring. + + The terrain has a ground with a floating ring in the middle. The ring extends from the center from + :obj:`platform_width` to :obj:`platform_width` + :obj:`ring_width` in the x and y directions. + The thickness of the ring is :obj:`ring_thickness` and the height of the ring from the terrain + is :obj:`ring_height`. + + .. image:: ../../_static/terrains/trimesh/floating_ring_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + """ + # resolve the terrain configuration + ring_height = cfg.ring_height_range[1] - difficulty * (cfg.ring_height_range[1] - cfg.ring_height_range[0]) + ring_width = cfg.ring_width_range[0] + difficulty * (cfg.ring_width_range[1] - cfg.ring_width_range[0]) + + # initialize list of meshes + meshes_list = list() + # constants for terrain generation + terrain_height = 1.0 + + # Generate the floating ring + ring_center = (0.5 * cfg.size[0], 0.5 * cfg.size[1], ring_height + 0.5 * cfg.ring_thickness) + ring_outer_size = (cfg.platform_width + 2 * ring_width, cfg.platform_width + 2 * ring_width) + ring_inner_size = (cfg.platform_width, cfg.platform_width) + meshes_list += make_border(ring_outer_size, ring_inner_size, cfg.ring_thickness, ring_center) + # Generate the ground + dim = (cfg.size[0], cfg.size[1], terrain_height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -terrain_height / 2) + ground = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(ground) + + # specify the origin of the terrain + origin = np.asarray([pos[0], pos[1], 0.0]) + + return meshes_list, origin + + +def star_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshStarTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a star. + + The terrain has a ground with a cylinder in the middle. The star is made of :obj:`num_bars` bars + with a width of :obj:`bar_width` and a height of :obj:`bar_height`. The bars are evenly + spaced around the cylinder and connect to the peripheral of the terrain. + + .. image:: ../../_static/terrains/trimesh/star_terrain.jpg + :width: 40% + :align: center + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + + Raises: + ValueError: If :obj:`num_bars` is less than 2. + """ + # check the number of bars + if cfg.num_bars < 2: + raise ValueError(f"The number of bars in the star must be greater than 2. Received: {cfg.num_bars}") + + # resolve the terrain configuration + bar_height = cfg.bar_height_range[0] + difficulty * (cfg.bar_height_range[1] - cfg.bar_height_range[0]) + bar_width = cfg.bar_width_range[1] - difficulty * (cfg.bar_width_range[1] - cfg.bar_width_range[0]) + + # initialize list of meshes + meshes_list = list() + # Generate a platform in the middle + platform_center = (0.5 * cfg.size[0], 0.5 * cfg.size[1], -bar_height / 2) + platform_transform = trimesh.transformations.translation_matrix(platform_center) + platform = trimesh.creation.cylinder( + cfg.platform_width * 0.5, bar_height, sections=2 * cfg.num_bars, transform=platform_transform + ) + meshes_list.append(platform) + # Generate bars to connect the platform to the terrain + transform = np.eye(4) + transform[:3, -1] = np.asarray(platform_center) + yaw = 0.0 + for _ in range(cfg.num_bars): + # compute the length of the bar based on the yaw + # length changes since the bar is connected to a square border + bar_length = cfg.size[0] + if yaw < 0.25 * np.pi: + bar_length /= np.math.cos(yaw) + elif yaw < 0.75 * np.pi: + bar_length /= np.math.sin(yaw) + else: + bar_length /= np.math.cos(np.pi - yaw) + # compute the transform of the bar + transform[0:3, 0:3] = tf.Rotation.from_euler("z", yaw).as_matrix() + # add the bar to the mesh + dim = [bar_length - bar_width, bar_width, bar_height] + bar = trimesh.creation.box(dim, transform) + meshes_list.append(bar) + # increment the yaw + yaw += np.pi / cfg.num_bars + # Generate the exterior border + inner_size = (cfg.size[0] - 2 * bar_width, cfg.size[1] - 2 * bar_width) + meshes_list += make_border(cfg.size, inner_size, bar_height, platform_center) + # Generate the ground + ground = make_plane(cfg.size, -bar_height, center_zero=False) + meshes_list.append(ground) + # specify the origin of the terrain + origin = np.asarray([0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.0]) + + return meshes_list, origin + + +def repeated_objects_terrain( + difficulty: float, cfg: mesh_terrains_cfg.MeshRepeatedObjectsTerrainCfg +) -> tuple[list[trimesh.Trimesh], np.ndarray]: + """Generate a terrain with a set of repeated objects. + + The terrain has a ground with a platform in the middle. The objects are randomly placed on the + terrain s.t. they do not overlap with the platform. + + Depending on the object type, the objects are generated with different parameters. The objects + The types of objects that can be generated are: ``"cylinder"``, ``"box"``, ``"cone"``. + + The object parameters are specified in the configuration as curriculum parameters. The difficulty + is used to linearly interpolate between the minimum and maximum values of the parameters. + + .. image:: ../../_static/terrains/trimesh/repeated_objects_cylinder_terrain.jpg + :width: 30% + + .. image:: ../../_static/terrains/trimesh/repeated_objects_box_terrain.jpg + :width: 30% + + .. image:: ../../_static/terrains/trimesh/repeated_objects_pyramid_terrain.jpg + :width: 30% + + Args: + difficulty: The difficulty of the terrain. This is a value between 0 and 1. + cfg: The configuration for the terrain. + + Returns: + A tuple containing the tri-mesh of the terrain and the origin of the terrain (in m). + + Raises: + ValueError: If the object type is not supported. It must be either a string or a callable. + """ + # import the object functions -- this is done here to avoid circular imports + from .mesh_terrains_cfg import ( + MeshRepeatedBoxesTerrainCfg, + MeshRepeatedCylindersTerrainCfg, + MeshRepeatedPyramidsTerrainCfg, + ) + + # if object type is a string, get the function: make_{object_type} + if isinstance(cfg.object_type, str): + object_func = globals().get(f"make_{cfg.object_type}") + else: + object_func = cfg.object_type + if not callable(object_func): + raise ValueError(f"The attribute 'object_type' must be a string or a callable. Received: {object_func}") + + # Resolve the terrain configuration + # -- pass parameters to make calling simpler + cp_0 = cfg.object_params_start + cp_1 = cfg.object_params_end + # -- common parameters + num_objects = cp_0.num_objects + int(difficulty * (cp_1.num_objects - cp_0.num_objects)) + height = cp_0.height + difficulty * (cp_1.height - cp_0.height) + # -- object specific parameters + # note: SIM114 requires duplicated logical blocks under a single body. + if isinstance(cfg, MeshRepeatedBoxesTerrainCfg): + cp_0: MeshRepeatedBoxesTerrainCfg.ObjectCfg + cp_1: MeshRepeatedBoxesTerrainCfg.ObjectCfg + object_kwargs = { + "length": cp_0.size[0] + difficulty * (cp_1.size[0] - cp_0.size[0]), + "width": cp_0.size[1] + difficulty * (cp_1.size[1] - cp_0.size[1]), + "max_yx_angle": cp_0.max_yx_angle + difficulty * (cp_1.max_yx_angle - cp_0.max_yx_angle), + "degrees": cp_0.degrees, + } + elif isinstance(cfg, MeshRepeatedPyramidsTerrainCfg): # noqa: SIM114 + cp_0: MeshRepeatedPyramidsTerrainCfg.ObjectCfg + cp_1: MeshRepeatedPyramidsTerrainCfg.ObjectCfg + object_kwargs = { + "radius": cp_0.radius + difficulty * (cp_1.radius - cp_0.radius), + "max_yx_angle": cp_0.max_yx_angle + difficulty * (cp_1.max_yx_angle - cp_0.max_yx_angle), + "degrees": cp_0.degrees, + } + elif isinstance(cfg, MeshRepeatedCylindersTerrainCfg): # noqa: SIM114 + cp_0: MeshRepeatedCylindersTerrainCfg.ObjectCfg + cp_1: MeshRepeatedCylindersTerrainCfg.ObjectCfg + object_kwargs = { + "radius": cp_0.radius + difficulty * (cp_1.radius - cp_0.radius), + "max_yx_angle": cp_0.max_yx_angle + difficulty * (cp_1.max_yx_angle - cp_0.max_yx_angle), + "degrees": cp_0.degrees, + } + else: + raise ValueError(f"Unknown terrain configuration: {cfg}") + # constants for the terrain + platform_clearance = 0.1 + + # initialize list of meshes + meshes_list = list() + # compute quantities + origin = np.asarray((0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.5 * height)) + platform_corners = np.asarray([ + [origin[0] - cfg.platform_width / 2, origin[1] - cfg.platform_width / 2], + [origin[0] + cfg.platform_width / 2, origin[1] + cfg.platform_width / 2], + ]) + platform_corners[0, :] *= 1 - platform_clearance + platform_corners[1, :] *= 1 + platform_clearance + # sample valid center for objects + object_centers = np.zeros((num_objects, 3)) + # use a mask to track invalid objects that still require sampling + mask_objects_left = np.ones((num_objects,), dtype=bool) + # loop until no objects are left to sample + while np.any(mask_objects_left): + # only sample the centers of the remaining invalid objects + num_objects_left = mask_objects_left.sum() + object_centers[mask_objects_left, 0] = np.random.uniform(0, cfg.size[0], num_objects_left) + object_centers[mask_objects_left, 1] = np.random.uniform(0, cfg.size[1], num_objects_left) + # filter out the centers that are on the platform + is_within_platform_x = np.logical_and( + object_centers[mask_objects_left, 0] >= platform_corners[0, 0], + object_centers[mask_objects_left, 0] <= platform_corners[1, 0], + ) + is_within_platform_y = np.logical_and( + object_centers[mask_objects_left, 1] >= platform_corners[0, 1], + object_centers[mask_objects_left, 1] <= platform_corners[1, 1], + ) + # update the mask to track the validity of the objects sampled in this iteration + mask_objects_left[mask_objects_left] = np.logical_and(is_within_platform_x, is_within_platform_y) + + # generate obstacles (but keep platform clean) + for index in range(len(object_centers)): + # randomize the height of the object + ob_height = height + np.random.uniform(-cfg.max_height_noise, cfg.max_height_noise) + if ob_height > 0.0: + object_mesh = object_func(center=object_centers[index], height=ob_height, **object_kwargs) + meshes_list.append(object_mesh) + + # generate a ground plane for the terrain + ground_plane = make_plane(cfg.size, height=0.0, center_zero=False) + meshes_list.append(ground_plane) + # generate a platform in the middle + dim = (cfg.platform_width, cfg.platform_width, 0.5 * height) + pos = (0.5 * cfg.size[0], 0.5 * cfg.size[1], 0.25 * height) + platform = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) + meshes_list.append(platform) + + return meshes_list, origin diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains_cfg.py new file mode 100644 index 00000000000..187e3698182 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/mesh_terrains_cfg.py @@ -0,0 +1,274 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +from typing import Literal + +import isaaclab.terrains.trimesh.mesh_terrains as mesh_terrains +import isaaclab.terrains.trimesh.utils as mesh_utils_terrains +from isaaclab.utils import configclass + +from ..terrain_generator_cfg import SubTerrainBaseCfg + +""" +Different trimesh terrain configurations. +""" + + +@configclass +class MeshPlaneTerrainCfg(SubTerrainBaseCfg): + """Configuration for a plane mesh terrain.""" + + function = mesh_terrains.flat_terrain + + +@configclass +class MeshPyramidStairsTerrainCfg(SubTerrainBaseCfg): + """Configuration for a pyramid stair mesh terrain.""" + + function = mesh_terrains.pyramid_stairs_terrain + + border_width: float = 0.0 + """The width of the border around the terrain (in m). Defaults to 0.0. + + The border is a flat terrain with the same height as the terrain. + """ + step_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the steps (in m).""" + step_width: float = MISSING + """The width of the steps (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + holes: bool = False + """If True, the terrain will have holes in the steps. Defaults to False. + + If :obj:`holes` is True, the terrain will have pyramid stairs of length or width + :obj:`platform_width` (depending on the direction) with no steps in the remaining area. Additionally, + no border will be added. + """ + + +@configclass +class MeshInvertedPyramidStairsTerrainCfg(MeshPyramidStairsTerrainCfg): + """Configuration for an inverted pyramid stair mesh terrain. + + Note: + This is the same as :class:`MeshPyramidStairsTerrainCfg` except that the steps are inverted. + """ + + function = mesh_terrains.inverted_pyramid_stairs_terrain + + +@configclass +class MeshRandomGridTerrainCfg(SubTerrainBaseCfg): + """Configuration for a random grid mesh terrain.""" + + function = mesh_terrains.random_grid_terrain + + grid_width: float = MISSING + """The width of the grid cells (in m).""" + grid_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the grid cells (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + holes: bool = False + """If True, the terrain will have holes in the steps. Defaults to False. + + If :obj:`holes` is True, the terrain will have randomized grid cells only along the plane extending + from the platform (like a plus sign). The remaining area remains empty and no border will be added. + """ + + +@configclass +class MeshRailsTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with box rails as extrusions.""" + + function = mesh_terrains.rails_terrain + + rail_thickness_range: tuple[float, float] = MISSING + """The thickness of the inner and outer rails (in m).""" + rail_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the rails (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class MeshPitTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with a pit that leads out of the pit.""" + + function = mesh_terrains.pit_terrain + + pit_depth_range: tuple[float, float] = MISSING + """The minimum and maximum height of the pit (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + double_pit: bool = False + """If True, the pit contains two levels of stairs. Defaults to False.""" + + +@configclass +class MeshBoxTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with boxes (similar to a pyramid).""" + + function = mesh_terrains.box_terrain + + box_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the box (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + double_box: bool = False + """If True, the pit contains two levels of stairs/boxes. Defaults to False.""" + + +@configclass +class MeshGapTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with a gap around the platform.""" + + function = mesh_terrains.gap_terrain + + gap_width_range: tuple[float, float] = MISSING + """The minimum and maximum width of the gap (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class MeshFloatingRingTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with a floating ring around the center.""" + + function = mesh_terrains.floating_ring_terrain + + ring_width_range: tuple[float, float] = MISSING + """The minimum and maximum width of the ring (in m).""" + ring_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the ring (in m).""" + ring_thickness: float = MISSING + """The thickness (along z) of the ring (in m).""" + platform_width: float = 1.0 + """The width of the square platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class MeshStarTerrainCfg(SubTerrainBaseCfg): + """Configuration for a terrain with a star pattern.""" + + function = mesh_terrains.star_terrain + + num_bars: int = MISSING + """The number of bars per-side the star. Must be greater than 2.""" + bar_width_range: tuple[float, float] = MISSING + """The minimum and maximum width of the bars in the star (in m).""" + bar_height_range: tuple[float, float] = MISSING + """The minimum and maximum height of the bars in the star (in m).""" + platform_width: float = 1.0 + """The width of the cylindrical platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class MeshRepeatedObjectsTerrainCfg(SubTerrainBaseCfg): + """Base configuration for a terrain with repeated objects.""" + + @configclass + class ObjectCfg: + """Configuration of repeated objects.""" + + num_objects: int = MISSING + """The number of objects to add to the terrain.""" + height: float = MISSING + """The height (along z) of the object (in m).""" + + function = mesh_terrains.repeated_objects_terrain + + object_type: Literal["cylinder", "box", "cone"] | callable = MISSING + """The type of object to generate. + + The type can be a string or a callable. If it is a string, the function will look for a function called + ``make_{object_type}`` in the current module scope. If it is a callable, the function will + use the callable to generate the object. + """ + object_params_start: ObjectCfg = MISSING + """The object curriculum parameters at the start of the curriculum.""" + object_params_end: ObjectCfg = MISSING + """The object curriculum parameters at the end of the curriculum.""" + + max_height_noise: float = 0.0 + """The maximum amount of noise to add to the height of the objects (in m). Defaults to 0.0.""" + platform_width: float = 1.0 + """The width of the cylindrical platform at the center of the terrain. Defaults to 1.0.""" + + +@configclass +class MeshRepeatedPyramidsTerrainCfg(MeshRepeatedObjectsTerrainCfg): + """Configuration for a terrain with repeated pyramids.""" + + @configclass + class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): + """Configuration for a curriculum of repeated pyramids.""" + + radius: float = MISSING + """The radius of the pyramids (in m).""" + max_yx_angle: float = 0.0 + """The maximum angle along the y and x axis. Defaults to 0.0.""" + degrees: bool = True + """Whether the angle is in degrees. Defaults to True.""" + + object_type = mesh_utils_terrains.make_cone + + object_params_start: ObjectCfg = MISSING + """The object curriculum parameters at the start of the curriculum.""" + object_params_end: ObjectCfg = MISSING + """The object curriculum parameters at the end of the curriculum.""" + + +@configclass +class MeshRepeatedBoxesTerrainCfg(MeshRepeatedObjectsTerrainCfg): + """Configuration for a terrain with repeated boxes.""" + + @configclass + class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): + """Configuration for repeated boxes.""" + + size: tuple[float, float] = MISSING + """The width (along x) and length (along y) of the box (in m).""" + max_yx_angle: float = 0.0 + """The maximum angle along the y and x axis. Defaults to 0.0.""" + degrees: bool = True + """Whether the angle is in degrees. Defaults to True.""" + + object_type = mesh_utils_terrains.make_box + + object_params_start: ObjectCfg = MISSING + """The box curriculum parameters at the start of the curriculum.""" + object_params_end: ObjectCfg = MISSING + """The box curriculum parameters at the end of the curriculum.""" + + +@configclass +class MeshRepeatedCylindersTerrainCfg(MeshRepeatedObjectsTerrainCfg): + """Configuration for a terrain with repeated cylinders.""" + + @configclass + class ObjectCfg(MeshRepeatedObjectsTerrainCfg.ObjectCfg): + """Configuration for repeated cylinder.""" + + radius: float = MISSING + """The radius of the pyramids (in m).""" + max_yx_angle: float = 0.0 + """The maximum angle along the y and x axis. Defaults to 0.0.""" + degrees: bool = True + """Whether the angle is in degrees. Defaults to True.""" + + object_type = mesh_utils_terrains.make_cylinder + + object_params_start: ObjectCfg = MISSING + """The box curriculum parameters at the start of the curriculum.""" + object_params_end: ObjectCfg = MISSING + """The box curriculum parameters at the end of the curriculum.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/utils.py new file mode 100644 index 00000000000..75ee68d554a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/trimesh/utils.py @@ -0,0 +1,199 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import scipy.spatial.transform as tf +import trimesh + +""" +Primitive functions to generate meshes. +""" + + +def make_plane(size: tuple[float, float], height: float, center_zero: bool = True) -> trimesh.Trimesh: + """Generate a plane mesh. + + If :obj:`center_zero` is True, the origin is at center of the plane mesh i.e. the mesh extends from + :math:`(-size[0] / 2, -size[1] / 2, 0)` to :math:`(size[0] / 2, size[1] / 2, height)`. + Otherwise, the origin is :math:`(size[0] / 2, size[1] / 2)` and the mesh extends from + :math:`(0, 0, 0)` to :math:`(size[0], size[1], height)`. + + Args: + size: The length (along x) and width (along y) of the terrain (in m). + height: The height of the plane (in m). + center_zero: Whether the 2D origin of the plane is set to the center of mesh. + Defaults to True. + + Returns: + A trimesh.Trimesh objects for the plane. + """ + # compute the vertices of the terrain + x0 = [size[0], size[1], height] + x1 = [size[0], 0.0, height] + x2 = [0.0, size[1], height] + x3 = [0.0, 0.0, height] + # generate the tri-mesh with two triangles + vertices = np.array([x0, x1, x2, x3]) + faces = np.array([[1, 0, 2], [2, 3, 1]]) + plane_mesh = trimesh.Trimesh(vertices=vertices, faces=faces) + # center the plane at the origin + if center_zero: + plane_mesh.apply_translation(-np.array([size[0] / 2.0, size[1] / 2.0, 0.0])) + # return the tri-mesh and the position + return plane_mesh + + +def make_border( + size: tuple[float, float], inner_size: tuple[float, float], height: float, position: tuple[float, float, float] +) -> list[trimesh.Trimesh]: + """Generate meshes for a rectangular border with a hole in the middle. + + .. code:: text + + +---------------------+ + |#####################| + |##+---------------+##| + |##| |##| + |##| |##| length + |##| |##| (y-axis) + |##| |##| + |##+---------------+##| + |#####################| + +---------------------+ + width (x-axis) + + Args: + size: The length (along x) and width (along y) of the terrain (in m). + inner_size: The inner length (along x) and width (along y) of the hole (in m). + height: The height of the border (in m). + position: The center of the border (in m). + + Returns: + A list of trimesh.Trimesh objects that represent the border. + """ + # compute thickness of the border + thickness_x = (size[0] - inner_size[0]) / 2.0 + thickness_y = (size[1] - inner_size[1]) / 2.0 + # generate tri-meshes for the border + # top/bottom border + box_dims = (size[0], thickness_y, height) + # -- top + box_pos = (position[0], position[1] + inner_size[1] / 2.0 + thickness_y / 2.0, position[2]) + box_mesh_top = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- bottom + box_pos = (position[0], position[1] - inner_size[1] / 2.0 - thickness_y / 2.0, position[2]) + box_mesh_bottom = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # left/right border + box_dims = (thickness_x, inner_size[1], height) + # -- left + box_pos = (position[0] - inner_size[0] / 2.0 - thickness_x / 2.0, position[1], position[2]) + box_mesh_left = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # -- right + box_pos = (position[0] + inner_size[0] / 2.0 + thickness_x / 2.0, position[1], position[2]) + box_mesh_right = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos)) + # return the tri-meshes + return [box_mesh_left, box_mesh_right, box_mesh_top, box_mesh_bottom] + + +def make_box( + length: float, + width: float, + height: float, + center: tuple[float, float, float], + max_yx_angle: float = 0, + degrees: bool = True, +) -> trimesh.Trimesh: + """Generate a box mesh with a random orientation. + + Args: + length: The length (along x) of the box (in m). + width: The width (along y) of the box (in m). + height: The height of the cylinder (in m). + center: The center of the cylinder (in m). + max_yx_angle: The maximum angle along the y and x axis. Defaults to 0. + degrees: Whether the angle is in degrees. Defaults to True. + + Returns: + A trimesh.Trimesh object for the cylinder. + """ + # create a pose for the cylinder + transform = np.eye(4) + transform[0:3, -1] = np.asarray(center) + # -- create a random rotation + euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,) + # -- cap the rotation along the y and x axis + if degrees: + max_yx_angle = max_yx_angle / 180.0 + euler_zyx[1:] *= max_yx_angle + # -- apply the rotation + transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix() + # create the box + dims = (length, width, height) + return trimesh.creation.box(dims, transform=transform) + + +def make_cylinder( + radius: float, height: float, center: tuple[float, float, float], max_yx_angle: float = 0, degrees: bool = True +) -> trimesh.Trimesh: + """Generate a cylinder mesh with a random orientation. + + Args: + radius: The radius of the cylinder (in m). + height: The height of the cylinder (in m). + center: The center of the cylinder (in m). + max_yx_angle: The maximum angle along the y and x axis. Defaults to 0. + degrees: Whether the angle is in degrees. Defaults to True. + + Returns: + A trimesh.Trimesh object for the cylinder. + """ + # create a pose for the cylinder + transform = np.eye(4) + transform[0:3, -1] = np.asarray(center) + # -- create a random rotation + euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,) + # -- cap the rotation along the y and x axis + if degrees: + max_yx_angle = max_yx_angle / 180.0 + euler_zyx[1:] *= max_yx_angle + # -- apply the rotation + transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix() + # create the cylinder + return trimesh.creation.cylinder(radius, height, sections=np.random.randint(4, 6), transform=transform) + + +def make_cone( + radius: float, height: float, center: tuple[float, float, float], max_yx_angle: float = 0, degrees: bool = True +) -> trimesh.Trimesh: + """Generate a cone mesh with a random orientation. + + Args: + radius: The radius of the cone (in m). + height: The height of the cone (in m). + center: The center of the cone (in m). + max_yx_angle: The maximum angle along the y and x axis. Defaults to 0. + degrees: Whether the angle is in degrees. Defaults to True. + + Returns: + A trimesh.Trimesh object for the cone. + """ + # create a pose for the cylinder + transform = np.eye(4) + transform[0:3, -1] = np.asarray(center) + # -- create a random rotation + euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,) + # -- cap the rotation along the y and x axis + if degrees: + max_yx_angle = max_yx_angle / 180.0 + euler_zyx[1:] *= max_yx_angle + # -- apply the rotation + transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix() + # create the cone + return trimesh.creation.cone(radius, height, sections=np.random.randint(4, 6), transform=transform) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/utils.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/utils.py new file mode 100644 index 00000000000..124bcb97b3c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/terrains/utils.py @@ -0,0 +1,280 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# needed to import for allowing type-hinting: np.ndarray | torch.Tensor | None +from __future__ import annotations + +import numpy as np +import torch +import trimesh + +import warp as wp + +from isaaclab.utils.warp import raycast_mesh + + +def color_meshes_by_height(meshes: list[trimesh.Trimesh], **kwargs) -> trimesh.Trimesh: + """ + Color the vertices of a trimesh object based on the z-coordinate (height) of each vertex, + using the Turbo colormap. If the z-coordinates are all the same, the vertices will be colored + with a single color. + + Args: + meshes: A list of trimesh objects. + + Keyword Args: + color: A list of 3 integers in the range [0,255] representing the RGB + color of the mesh. Used when the z-coordinates of all vertices are the same. + Defaults to [172, 216, 230]. + color_map: The name of the color map to be used. Defaults to "turbo". + + Returns: + A trimesh object with the vertices colored based on the z-coordinate (height) of each vertex. + """ + # Combine all meshes into a single mesh + mesh = trimesh.util.concatenate(meshes) + # Get the z-coordinates of each vertex + heights = mesh.vertices[:, 2] + # Check if the z-coordinates are all the same + if np.max(heights) == np.min(heights): + # Obtain a single color: light blue + color = kwargs.pop("color", (172, 216, 230)) + color = np.asarray(color, dtype=np.uint8) + # Set the color for all vertices + mesh.visual.vertex_colors = color + else: + # Normalize the heights to [0,1] + heights_normalized = (heights - np.min(heights)) / (np.max(heights) - np.min(heights)) + # clip lower and upper bounds to have better color mapping + heights_normalized = np.clip(heights_normalized, 0.1, 0.9) + # Get the color for each vertex based on the height + color_map = kwargs.pop("color_map", "turbo") + colors = trimesh.visual.color.interpolate(heights_normalized, color_map=color_map) + # Set the vertex colors + mesh.visual.vertex_colors = colors + # Return the mesh + return mesh + + +def create_prim_from_mesh(prim_path: str, mesh: trimesh.Trimesh, **kwargs): + """Create a USD prim with mesh defined from vertices and triangles. + + The function creates a USD prim with a mesh defined from vertices and triangles. It performs the + following steps: + + - Create a USD Xform prim at the path :obj:`prim_path`. + - Create a USD prim with a mesh defined from the input vertices and triangles at the path :obj:`{prim_path}/mesh`. + - Assign a physics material to the mesh at the path :obj:`{prim_path}/physicsMaterial`. + - Assign a visual material to the mesh at the path :obj:`{prim_path}/visualMaterial`. + + Args: + prim_path: The path to the primitive to be created. + mesh: The mesh to be used for the primitive. + + Keyword Args: + translation: The translation of the terrain. Defaults to None. + orientation: The orientation of the terrain. Defaults to None. + visual_material: The visual material to apply. Defaults to None. + physics_material: The physics material to apply. Defaults to None. + """ + # need to import these here to prevent isaacsim launching when importing this module + import isaacsim.core.utils.prims as prim_utils + from pxr import UsdGeom + + import isaaclab.sim as sim_utils + + # create parent prim + prim_utils.create_prim(prim_path, "Xform") + # create mesh prim + prim = prim_utils.create_prim( + f"{prim_path}/mesh", + "Mesh", + translation=kwargs.get("translation"), + orientation=kwargs.get("orientation"), + attributes={ + "points": mesh.vertices, + "faceVertexIndices": mesh.faces.flatten(), + "faceVertexCounts": np.asarray([3] * len(mesh.faces)), + "subdivisionScheme": "bilinear", + }, + ) + # apply collider properties + collider_cfg = sim_utils.CollisionPropertiesCfg(collision_enabled=True) + sim_utils.define_collision_properties(prim.GetPrimPath(), collider_cfg) + # add rgba color to the mesh primvars + if mesh.visual.vertex_colors is not None: + # obtain color from the mesh + rgba_colors = np.asarray(mesh.visual.vertex_colors).astype(np.float32) / 255.0 + # displayColor is a primvar attribute that is used to color the mesh + color_prim_attr = prim.GetAttribute("primvars:displayColor") + color_prim_var = UsdGeom.Primvar(color_prim_attr) + color_prim_var.SetInterpolation(UsdGeom.Tokens.vertex) + color_prim_attr.Set(rgba_colors[:, :3]) + # displayOpacity is a primvar attribute that is used to set the opacity of the mesh + display_prim_attr = prim.GetAttribute("primvars:displayOpacity") + display_prim_var = UsdGeom.Primvar(display_prim_attr) + display_prim_var.SetInterpolation(UsdGeom.Tokens.vertex) + display_prim_var.Set(rgba_colors[:, 3]) + + # create visual material + if kwargs.get("visual_material") is not None: + visual_material_cfg: sim_utils.VisualMaterialCfg = kwargs.get("visual_material") + # spawn the material + visual_material_cfg.func(f"{prim_path}/visualMaterial", visual_material_cfg) + sim_utils.bind_visual_material(prim.GetPrimPath(), f"{prim_path}/visualMaterial") + # create physics material + if kwargs.get("physics_material") is not None: + physics_material_cfg: sim_utils.RigidBodyMaterialCfg = kwargs.get("physics_material") + # spawn the material + physics_material_cfg.func(f"{prim_path}/physicsMaterial", physics_material_cfg) + sim_utils.bind_physics_material(prim.GetPrimPath(), f"{prim_path}/physicsMaterial") + + +def find_flat_patches( + wp_mesh: wp.Mesh, + num_patches: int, + patch_radius: float | list[float], + origin: np.ndarray | torch.Tensor | tuple[float, float, float], + x_range: tuple[float, float], + y_range: tuple[float, float], + z_range: tuple[float, float], + max_height_diff: float, +) -> torch.Tensor: + """Finds flat patches of given radius in the input mesh. + + The function finds flat patches of given radius based on the search space defined by the input ranges. + The search space is characterized by origin in the mesh frame, and the x, y, and z ranges. The x and y + ranges are used to sample points in the 2D region around the origin, and the z range is used to filter + patches based on the height of the points. + + The function performs rejection sampling to find the patches based on the following steps: + + 1. Sample patch locations in the 2D region around the origin. + 2. Define a ring of points around each patch location to query the height of the points using ray-casting. + 3. Reject patches that are outside the z range or have a height difference that is too large. + 4. Keep sampling until all patches are valid. + + Args: + wp_mesh: The warp mesh to find patches in. + num_patches: The desired number of patches to find. + patch_radius: The radii used to form patches. If a list is provided, multiple patch sizes are checked. + This is useful to deal with holes or other artifacts in the mesh. + origin: The origin defining the center of the search space. This is specified in the mesh frame. + x_range: The range of X coordinates to sample from. + y_range: The range of Y coordinates to sample from. + z_range: The range of valid Z coordinates used for filtering patches. + max_height_diff: The maximum allowable distance between the lowest and highest points + on a patch to consider it as valid. If the difference is greater than this value, + the patch is rejected. + + Returns: + A tensor of shape (num_patches, 3) containing the flat patches. The patches are defined in the mesh frame. + + Raises: + RuntimeError: If the function fails to find valid patches. This can happen if the input parameters + are not suitable for finding valid patches and maximum number of iterations is reached. + """ + # set device to warp mesh device + device = wp.device_to_torch(wp_mesh.device) + + # resolve inputs to consistent type + # -- patch radii + if isinstance(patch_radius, float): + patch_radius = [patch_radius] + # -- origin + if isinstance(origin, np.ndarray): + origin = torch.from_numpy(origin).to(torch.float).to(device) + elif isinstance(origin, torch.Tensor): + origin = origin.to(device) + else: + origin = torch.tensor(origin, dtype=torch.float, device=device) + + # create ranges for the x and y coordinates around the origin. + # The provided ranges are bounded by the mesh's bounding box. + x_range = ( + max(x_range[0] + origin[0].item(), wp_mesh.points.numpy()[:, 0].min()), + min(x_range[1] + origin[0].item(), wp_mesh.points.numpy()[:, 0].max()), + ) + y_range = ( + max(y_range[0] + origin[1].item(), wp_mesh.points.numpy()[:, 1].min()), + min(y_range[1] + origin[1].item(), wp_mesh.points.numpy()[:, 1].max()), + ) + z_range = ( + z_range[0] + origin[2].item(), + z_range[1] + origin[2].item(), + ) + + # create a circle of points around (0, 0) to query validity of the patches + # the ring of points is uniformly distributed around the circle + angle = torch.linspace(0, 2 * np.pi, 10, device=device) + query_x = [] + query_y = [] + for radius in patch_radius: + query_x.append(radius * torch.cos(angle)) + query_y.append(radius * torch.sin(angle)) + query_x = torch.cat(query_x).unsqueeze(1) # dim: (num_radii * 10, 1) + query_y = torch.cat(query_y).unsqueeze(1) # dim: (num_radii * 10, 1) + # dim: (num_radii * 10, 3) + query_points = torch.cat([query_x, query_y, torch.zeros_like(query_x)], dim=-1) + + # create buffers + # -- a buffer to store indices of points that are not valid + points_ids = torch.arange(num_patches, device=device) + # -- a buffer to store the flat patches locations + flat_patches = torch.zeros(num_patches, 3, device=device) + + # sample points and raycast to find the height. + # 1. Reject points that are outside the z_range or have a height difference that is too large. + # 2. Keep sampling until all points are valid. + iter_count = 0 + while len(points_ids) > 0 and iter_count < 10000: + # sample points in the 2D region around the origin + pos_x = torch.empty(len(points_ids), device=device).uniform_(*x_range) + pos_y = torch.empty(len(points_ids), device=device).uniform_(*y_range) + flat_patches[points_ids, :2] = torch.stack([pos_x, pos_y], dim=-1) + + # define the query points to check validity of the patch + # dim: (num_patches, num_radii * 10, 3) + points = flat_patches[points_ids].unsqueeze(1) + query_points + points[..., 2] = 100.0 + # ray-cast direction is downwards + dirs = torch.zeros_like(points) + dirs[..., 2] = -1.0 + + # ray-cast to find the height of the patches + ray_hits = raycast_mesh(points.view(-1, 3), dirs.view(-1, 3), wp_mesh)[0] + heights = ray_hits.view(points.shape)[..., 2] + # set the height of the patches + # note: for invalid patches, they would be overwritten in the next iteration + # so it's safe to set the height to the last value + flat_patches[points_ids, 2] = heights[..., -1] + + # check validity + # -- height is within the z range + not_valid = torch.any(torch.logical_or(heights < z_range[0], heights > z_range[1]), dim=1) + # -- height difference is within the max height difference + not_valid = torch.logical_or(not_valid, (heights.max(dim=1)[0] - heights.min(dim=1)[0]) > max_height_diff) + + # remove invalid patches indices + points_ids = points_ids[not_valid] + # increment count + iter_count += 1 + + # check all patches are valid + if len(points_ids) > 0: + raise RuntimeError( + "Failed to find valid patches! Please check the input parameters." + f"\n\tMaximum number of iterations reached: {iter_count}" + f"\n\tNumber of invalid patches: {len(points_ids)}" + f"\n\tMaximum height difference: {max_height_diff}" + ) + + # return the flat patches (in the mesh frame) + return flat_patches - origin diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/__init__.py new file mode 100644 index 00000000000..f3870f82c30 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .image_plot import ImagePlot +from .line_plot import LiveLinePlot +from .manager_live_visualizer import ManagerLiveVisualizer +from .ui_visualizer_base import UiVisualizerBase diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/image_plot.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/image_plot.py new file mode 100644 index 00000000000..ff924c72ad3 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/image_plot.py @@ -0,0 +1,213 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from contextlib import suppress +from matplotlib import cm +from typing import TYPE_CHECKING, Optional + +import carb +import omni +import omni.log + +with suppress(ImportError): + # isaacsim.gui is not available when running in headless mode. + import isaacsim.gui.components.ui_utils + +from .ui_widget_wrapper import UIWidgetWrapper + +if TYPE_CHECKING: + import isaacsim.gui.components + import omni.ui + + +class ImagePlot(UIWidgetWrapper): + """An image plot widget to display live data. + + It has the following Layout where the mode frame is only useful for depth images: + +-------------------------------------------------------+ + | containing_frame | + |+-----------------------------------------------------+| + | main_plot_frame | + ||+---------------------------------------------------+|| + ||| plot_frames ||| + ||| ||| + ||| ||| + ||| (Image Plot Data) ||| + ||| ||| + ||| ||| + |||+-------------------------------------------------+||| + ||| mode_frame ||| + ||| ||| + ||| [x][Absolute] [x][Grayscaled] [ ][Colorized] ||| + |+-----------------------------------------------------+| + +-------------------------------------------------------+ + + """ + + def __init__( + self, + image: Optional[np.ndarray] = None, + label: str = "", + widget_height: int = 200, + show_min_max: bool = True, + unit: tuple[float, str] = (1, ""), + ): + """Create an XY plot UI Widget with axis scaling, legends, and support for multiple plots. + + Overlapping data is most accurately plotted when centered in the frame with reasonable axis scaling. + Pressing down the mouse gives the x and y values of each function at an x coordinate. + + Args: + image: Image to display + label: Short descriptive text to the left of the plot + widget_height: Height of the plot in pixels + show_min_max: Whether to show the min and max values of the image + unit: Tuple of (scale, name) for the unit of the image + """ + self._show_min_max = show_min_max + self._unit_scale = unit[0] + self._unit_name = unit[1] + + self._curr_mode = "None" + + self._has_built = False + + self._enabled = True + + self._byte_provider = omni.ui.ByteImageProvider() + if image is None: + carb.log_warn("image is NONE") + image = np.ones((480, 640, 3), dtype=np.uint8) * 255 + image[:, :, 0] = 0 + image[:, :240, 1] = 0 + + # if image is channel first, convert to channel last + if image.ndim == 3 and image.shape[0] in [1, 3, 4]: + image = np.moveaxis(image, 0, -1) + + self._aspect_ratio = image.shape[1] / image.shape[0] + self._widget_height = widget_height + self._label = label + self.update_image(image) + + plot_frame = self._create_ui_widget() + + super().__init__(plot_frame) + + def setEnabled(self, enabled: bool): + self._enabled = enabled + + def update_image(self, image: np.ndarray): + if not self._enabled: + return + + # if image is channel first, convert to channel last + if image.ndim == 3 and image.shape[0] in [1, 3, 4]: + image = np.moveaxis(image, 0, -1) + + height, width = image.shape[:2] + + if self._curr_mode == "Normalization": + image = (image - image.min()) / (image.max() - image.min()) + image = (image * 255).astype(np.uint8) + elif self._curr_mode == "Colorization": + if image.ndim == 3 and image.shape[2] == 3: + omni.log.warn("Colorization mode is only available for single channel images") + else: + image = (image - image.min()) / (image.max() - image.min()) + colormap = cm.get_cmap("jet") + if image.ndim == 3 and image.shape[2] == 1: + image = (colormap(image).squeeze(2) * 255).astype(np.uint8) + else: + image = (colormap(image) * 255).astype(np.uint8) + + # convert image to 4-channel RGBA + if image.ndim == 2 or (image.ndim == 3 and image.shape[2] == 1): + image = np.dstack((image, image, image, np.full((height, width, 1), 255, dtype=np.uint8))) + + elif image.ndim == 3 and image.shape[2] == 3: + image = np.dstack((image, np.full((height, width, 1), 255, dtype=np.uint8))) + + self._byte_provider.set_bytes_data(image.flatten().data, [width, height]) + + def update_min_max(self, image: np.ndarray): + if self._show_min_max and hasattr(self, "_min_max_label"): + non_inf = image[np.isfinite(image)].flatten() + if len(non_inf) > 0: + self._min_max_label.text = self._get_unit_description( + np.min(non_inf), np.max(non_inf), np.median(non_inf) + ) + else: + self._min_max_label.text = self._get_unit_description(0, 0) + + def _create_ui_widget(self): + containing_frame = omni.ui.Frame(build_fn=self._build_widget) + return containing_frame + + def _get_unit_description(self, min_value: float, max_value: float, median_value: float = None): + return ( + f"Min: {min_value * self._unit_scale:.2f} {self._unit_name} Max:" + f" {max_value * self._unit_scale:.2f} {self._unit_name}" + + (f" Median: {median_value * self._unit_scale:.2f} {self._unit_name}" if median_value is not None else "") + ) + + def _build_widget(self): + + with omni.ui.VStack(spacing=3): + with omni.ui.HStack(): + # Write the leftmost label for what this plot is + omni.ui.Label( + self._label, + width=isaacsim.gui.components.ui_utils.LABEL_WIDTH, + alignment=omni.ui.Alignment.LEFT_TOP, + ) + with omni.ui.Frame(width=self._aspect_ratio * self._widget_height, height=self._widget_height): + self._base_plot = omni.ui.ImageWithProvider(self._byte_provider) + + if self._show_min_max: + self._min_max_label = omni.ui.Label(self._get_unit_description(0, 0)) + + omni.ui.Spacer(height=8) + self._mode_frame = omni.ui.Frame(build_fn=self._build_mode_frame) + + omni.ui.Spacer(width=5) + self._has_built = True + + def _build_mode_frame(self): + """Build the frame containing the mode selection for the plots. + + This is an internal function to build the frame containing the mode selection for the plots. This function + should only be called from within the build function of a frame. + + The built widget has the following layout: + +-------------------------------------------------------+ + | legends_frame | + ||+---------------------------------------------------+|| + ||| ||| + ||| [x][Series 1] [x][Series 2] [ ][Series 3] ||| + ||| ||| + |||+-------------------------------------------------+||| + |+-----------------------------------------------------+| + +-------------------------------------------------------+ + """ + with omni.ui.HStack(): + with omni.ui.HStack(): + + def _change_mode(value): + self._curr_mode = value + + isaacsim.gui.components.ui_utils.dropdown_builder( + label="Mode", + type="dropdown", + items=["Original", "Normalization", "Colorization"], + tooltip="Select a mode", + on_clicked_fn=_change_mode, + ) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/line_plot.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/line_plot.py new file mode 100644 index 00000000000..d743efa428e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/line_plot.py @@ -0,0 +1,613 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import colorsys +import numpy as np +from contextlib import suppress +from typing import TYPE_CHECKING + +import omni +from isaacsim.core.api.simulation_context import SimulationContext + +with suppress(ImportError): + # isaacsim.gui is not available when running in headless mode. + import isaacsim.gui.components.ui_utils + +from .ui_widget_wrapper import UIWidgetWrapper + +if TYPE_CHECKING: + import isaacsim.gui.components + import omni.ui + + +class LiveLinePlot(UIWidgetWrapper): + """A 2D line plot widget to display live data. + + + This widget is used to display live data in a 2D line plot. It can be used to display multiple series + in the same plot. + + It has the following Layout: + +-------------------------------------------------------+ + | containing_frame | + |+-----------------------------------------------------+| + | main_plot_frame | + ||+---------------------------------------------------+|| + ||| plot_frames + grid lines (Z_stacked) ||| + ||| ||| + ||| ||| + ||| (Live Plot Data) ||| + ||| ||| + ||| ||| + |||+-------------------------------------------------+||| + ||| legends_frame ||| + ||| ||| + ||| [x][Series 1] [x][Series 2] [ ][Series 3] ||| + |||+-------------------------------------------------+||| + ||| limits_frame ||| + ||| ||| + ||| [Y-Limits] [min] [max] [Autoscale] ||| + |||+-------------------------------------------------+||| + ||| filter_frame ||| + ||| ||| + ||| ||| + |+-----------------------------------------------------+| + +-------------------------------------------------------+ + + """ + + def __init__( + self, + y_data: list[list[float]], + y_min: float = -10, + y_max: float = 10, + plot_height: int = 150, + show_legend: bool = True, + legends: list[str] | None = None, + max_datapoints: int = 200, + ): + """Create a new LiveLinePlot widget. + + Args: + y_data: A list of lists of floats containing the data to plot. Each list of floats represents a series in the plot. + y_min: The minimum y value to display. Defaults to -10. + y_max: The maximum y value to display. Defaults to 10. + plot_height: The height of the plot in pixels. Defaults to 150. + show_legend: Whether to display the legend. Defaults to True. + legends: A list of strings containing the legend labels for each series. If None, the default labels are "Series_0", "Series_1", etc. Defaults to None. + max_datapoints: The maximum number of data points to display. If the number of data points exceeds this value, the oldest data points are removed. Defaults to 200. + """ + super().__init__(self._create_ui_widget()) + self.plot_height = plot_height + self.show_legend = show_legend + self._legends = legends if legends is not None else ["Series_" + str(i) for i in range(len(y_data))] + self._y_data = y_data + self._colors = self._get_distinct_hex_colors(len(y_data)) + self._y_min = y_min if y_min is not None else -10 + self._y_max = y_max if y_max is not None else 10 + self._max_data_points = max_datapoints + self._show_legend = show_legend + self._series_visible = [True for _ in range(len(y_data))] + self._plot_frames = [] + self._plots = [] + self._plot_selected_values = [] + self._is_built = False + self._filter_frame = None + self._filter_mode = None + self._last_values = None + self._is_paused = False + + # Gets populated when widget is built + self._main_plot_frame = None + + self._autoscale_model = omni.ui.SimpleBoolModel(True) + + """Properties""" + + @property + def autoscale_mode(self) -> bool: + return self._autoscale_model.as_bool + + @property + def y_data(self) -> list[list[float]]: + """The current data in the plot.""" + return self._y_data + + @property + def y_min(self) -> float: + """The current minimum y value.""" + return self._y_min + + @property + def y_max(self) -> float: + """The current maximum y value.""" + return self._y_max + + @property + def legends(self) -> list[str]: + """The current legend labels.""" + return self._legends + + """ General Functions """ + + def clear(self): + """Clears the plot.""" + self._y_data = [[] for _ in range(len(self._y_data))] + self._last_values = None + + for plt in self._plots: + plt.set_data() + + # self._container_frame.rebuild() + + def add_datapoint(self, y_coords: list[float]): + """Add a data point to the plot. + + The data point is added to the end of the plot. If the number of data points exceeds the maximum number + of data points, the oldest data point is removed. + + ``y_coords`` is assumed to be a list of floats with the same length as the number of series in the plot. + + Args: + y_coords: A list of floats containing the y coordinates of the new data points. + """ + + for idx, y_coord in enumerate(y_coords): + + if len(self._y_data[idx]) > self._max_data_points: + self._y_data[idx] = self._y_data[idx][1:] + + if self._filter_mode == "Lowpass": + if self._last_values is not None: + alpha = 0.8 + y_coord = self._y_data[idx][-1] * alpha + y_coord * (1 - alpha) + elif self._filter_mode == "Integrate": + if self._last_values is not None: + y_coord = self._y_data[idx][-1] + y_coord + elif self._filter_mode == "Derivative": + if self._last_values is not None: + y_coord = (y_coord - self._last_values[idx]) / SimulationContext.instance().get_rendering_dt() + + self._y_data[idx].append(float(y_coord)) + + if self._main_plot_frame is None: + # Widget not built, not visible + return + + # Check if the widget has been built, i.e. the plot references have been created. + if not self._is_built or self._is_paused: + return + + if len(self._y_data) != len(self._plots): + # Plots gotten out of sync, rebuild the widget + self._main_plot_frame.rebuild() + return + + if self.autoscale_mode: + self._rescale_btn_pressed() + + for idx, plt in enumerate(self._plots): + plt.set_data(*self._y_data[idx]) + + self._last_values = y_coords + # Autoscale the y-axis to the current data + + """ + Internal functions for building the UI. + """ + + def _build_stacked_plots(self, grid: bool = True): + """Builds multiple plots stacked on top of each other to display multiple series. + + This is an internal function to build the plots. It should not be called from outside the class and only + from within the build function of a frame. + + The built widget has the following layout: + +-------------------------------------------------------+ + | main_plot_frame | + ||+---------------------------------------------------+|| + ||| ||| + ||| y_max|*******-------------------*******| ||| + ||| |-------*****-----------**--------| ||| + ||| 0|------------**-----***-----------| ||| + ||| |--------------***----------------| ||| + ||| y_min|---------------------------------| ||| + ||| ||| + |||+-------------------------------------------------+||| + + + Args: + grid: Whether to display grid lines. Defaults to True. + """ + + # Reset lists which are populated in the build function + self._plot_frames = [] + + # Define internal builder function + def _build_single_plot(y_data: list[float], color: int, plot_idx: int): + """Build a single plot. + + This is an internal function to build a single plot with the given data and color. This function + should only be called from within the build function of a frame. + + Args: + y_data: The data to plot. + color: The color of the plot. + """ + plot = omni.ui.Plot( + omni.ui.Type.LINE, + self._y_min, + self._y_max, + *y_data, + height=self.plot_height, + style={"color": color, "background_color": 0x0}, + ) + + if len(self._plots) <= plot_idx: + self._plots.append(plot) + self._plot_selected_values.append(omni.ui.SimpleStringModel("")) + else: + self._plots[plot_idx] = plot + + # Begin building the widget + with omni.ui.HStack(): + # Space to the left to add y-axis labels + omni.ui.Spacer(width=20) + + # Built plots for each time series stacked on top of each other + with omni.ui.ZStack(): + # Background rectangle + omni.ui.Rectangle( + height=self.plot_height, + style={ + "background_color": 0x0, + "border_color": omni.ui.color.white, + "border_width": 0.4, + "margin": 0.0, + }, + ) + + # Draw grid lines and labels + if grid: + # Calculate the number of grid lines to display + # Absolute range of the plot + plot_range = self._y_max - self._y_min + grid_resolution = 10 ** np.floor(np.log10(0.5 * plot_range)) + + plot_range /= grid_resolution + + # Fraction of the plot range occupied by the first and last grid line + first_space = (self._y_max / grid_resolution) - np.floor(self._y_max / grid_resolution) + last_space = np.ceil(self._y_min / grid_resolution) - self._y_min / grid_resolution + + # Number of grid lines to display + n_lines = int(plot_range - first_space - last_space) + + plot_resolution = self.plot_height / plot_range + + with omni.ui.VStack(): + omni.ui.Spacer(height=plot_resolution * first_space) + + # Draw grid lines + with omni.ui.VGrid(row_height=plot_resolution): + for grid_line_idx in range(n_lines): + # Create grid line + with omni.ui.ZStack(): + omni.ui.Line( + style={ + "color": 0xAA8A8777, + "background_color": 0x0, + "border_width": 0.4, + }, + alignment=omni.ui.Alignment.CENTER_TOP, + height=0, + ) + with omni.ui.Placer(offset_x=-20): + omni.ui.Label( + f"{(self._y_max - first_space * grid_resolution - grid_line_idx * grid_resolution):.3f}", + width=8, + height=8, + alignment=omni.ui.Alignment.RIGHT_TOP, + style={ + "color": 0xFFFFFFFF, + "font_size": 8, + }, + ) + + # Create plots for each series + for idx, (data, color) in enumerate(zip(self._y_data, self._colors)): + plot_frame = omni.ui.Frame( + build_fn=lambda y_data=data, plot_idx=idx, color=color: _build_single_plot( + y_data, color, plot_idx + ), + ) + plot_frame.visible = self._series_visible[idx] + self._plot_frames.append(plot_frame) + + # Create an invisible frame on top that will give a helpful tooltip + self._tooltip_frame = omni.ui.Plot( + height=self.plot_height, + style={"color": 0xFFFFFFFF, "background_color": 0x0}, + ) + + self._tooltip_frame.set_mouse_pressed_fn(self._mouse_moved_on_plot) + + # Create top label for the y-axis + with omni.ui.Placer(offset_x=-20, offset_y=-8): + omni.ui.Label( + f"{self._y_max:.3f}", + width=8, + height=2, + alignment=omni.ui.Alignment.LEFT_TOP, + style={"color": 0xFFFFFFFF, "font_size": 8}, + ) + + # Create bottom label for the y-axis + with omni.ui.Placer(offset_x=-20, offset_y=self.plot_height): + omni.ui.Label( + f"{self._y_min:.3f}", + width=8, + height=2, + alignment=omni.ui.Alignment.LEFT_BOTTOM, + style={"color": 0xFFFFFFFF, "font_size": 8}, + ) + + def _mouse_moved_on_plot(self, x, y, *args): + # Show a tooltip with x,y and function values + if len(self._y_data) == 0 or len(self._y_data[0]) == 0: + # There is no data in the plots, so do nothing + return + + for idx, plot in enumerate(self._plots): + x_pos = plot.screen_position_x + width = plot.computed_width + + location_x = (x - x_pos) / width + + data = self._y_data[idx] + n_samples = len(data) + selected_sample = int(location_x * n_samples) + value = data[selected_sample] + # save the value in scientific notation + self._plot_selected_values[idx].set_value(f"{value:.3f}") + + def _build_legends_frame(self): + """Build the frame containing the legend for the plots. + + This is an internal function to build the frame containing the legend for the plots. This function + should only be called from within the build function of a frame. + + The built widget has the following layout: + +-------------------------------------------------------+ + | legends_frame | + ||+---------------------------------------------------+|| + ||| ||| + ||| [x][Series 1] [x][Series 2] [ ][Series 3] ||| + ||| ||| + |||+-------------------------------------------------+||| + |+-----------------------------------------------------+| + +-------------------------------------------------------+ + """ + if not self._show_legend: + return + + with omni.ui.HStack(): + omni.ui.Spacer(width=32) + + # Find the longest legend to determine the width of the frame + max_legend = max([len(legend) for legend in self._legends]) + CHAR_WIDTH = 8 + with omni.ui.VGrid( + row_height=isaacsim.gui.components.ui_utils.LABEL_HEIGHT, + column_width=max_legend * CHAR_WIDTH + 6, + ): + for idx in range(len(self._y_data)): + with omni.ui.HStack(): + model = omni.ui.SimpleBoolModel() + model.set_value(self._series_visible[idx]) + omni.ui.CheckBox(model=model, tooltip="", width=4) + model.add_value_changed_fn(lambda val, idx=idx: self._change_plot_visibility(idx, val.as_bool)) + omni.ui.Spacer(width=2) + with omni.ui.VStack(): + omni.ui.Label( + self._legends[idx], + width=max_legend * CHAR_WIDTH, + alignment=omni.ui.Alignment.LEFT, + style={"color": self._colors[idx], "font_size": 12}, + ) + omni.ui.StringField( + model=self._plot_selected_values[idx], + width=max_legend * CHAR_WIDTH, + alignment=omni.ui.Alignment.LEFT, + style={"color": self._colors[idx], "font_size": 10}, + read_only=True, + ) + + def _build_limits_frame(self): + """Build the frame containing the controls for the y-axis limits. + + This is an internal function to build the frame containing the controls for the y-axis limits. This function + should only be called from within the build function of a frame. + + The built widget has the following layout: + +-------------------------------------------------------+ + | limits_frame | + ||+---------------------------------------------------+|| + ||| ||| + ||| Limits [min] [max] [Re-Sacle] ||| + ||| Autoscale[x] ||| + ||| ------------------------------------------- ||| + |||+-------------------------------------------------+||| + """ + with omni.ui.VStack(): + with omni.ui.HStack(): + omni.ui.Label( + "Limits", + width=isaacsim.gui.components.ui_utils.LABEL_WIDTH, + alignment=omni.ui.Alignment.LEFT_CENTER, + ) + + self.lower_limit_drag = omni.ui.FloatDrag(name="min", enabled=True, alignment=omni.ui.Alignment.CENTER) + y_min_model = self.lower_limit_drag.model + y_min_model.set_value(self._y_min) + y_min_model.add_value_changed_fn(lambda x: self._set_y_min(x.as_float)) + omni.ui.Spacer(width=2) + + self.upper_limit_drag = omni.ui.FloatDrag(name="max", enabled=True, alignment=omni.ui.Alignment.CENTER) + y_max_model = self.upper_limit_drag.model + y_max_model.set_value(self._y_max) + y_max_model.add_value_changed_fn(lambda x: self._set_y_max(x.as_float)) + omni.ui.Spacer(width=2) + + omni.ui.Button( + "Re-Scale", + width=isaacsim.gui.components.ui_utils.BUTTON_WIDTH, + clicked_fn=self._rescale_btn_pressed, + alignment=omni.ui.Alignment.LEFT_CENTER, + style=isaacsim.gui.components.ui_utils.get_style(), + ) + + omni.ui.CheckBox(model=self._autoscale_model, tooltip="", width=4) + + omni.ui.Line( + style={"color": 0x338A8777}, + width=omni.ui.Fraction(1), + alignment=omni.ui.Alignment.CENTER, + ) + + def _build_filter_frame(self): + """Build the frame containing the filter controls. + + This is an internal function to build the frame containing the filter controls. This function + should only be called from within the build function of a frame. + + The built widget has the following layout: + +-------------------------------------------------------+ + | filter_frame | + ||+---------------------------------------------------+|| + ||| ||| + ||| ||| + ||| ||| + |||+-------------------------------------------------+||| + |+-----------------------------------------------------+| + +-------------------------------------------------------+ + """ + with omni.ui.VStack(): + with omni.ui.HStack(): + + def _filter_changed(value): + self.clear() + self._filter_mode = value + + isaacsim.gui.components.ui_utils.dropdown_builder( + label="Filter", + type="dropdown", + items=["None", "Lowpass", "Integrate", "Derivative"], + tooltip="Select a filter", + on_clicked_fn=_filter_changed, + ) + + def _toggle_paused(): + self._is_paused = not self._is_paused + + # Button + omni.ui.Button( + "Play/Pause", + width=isaacsim.gui.components.ui_utils.BUTTON_WIDTH, + clicked_fn=_toggle_paused, + alignment=omni.ui.Alignment.LEFT_CENTER, + style=isaacsim.gui.components.ui_utils.get_style(), + ) + + def _create_ui_widget(self): + """Create the full UI widget.""" + + def _build_widget(): + self._is_built = False + with omni.ui.VStack(): + self._main_plot_frame = omni.ui.Frame(build_fn=self._build_stacked_plots) + omni.ui.Spacer(height=8) + self._legends_frame = omni.ui.Frame(build_fn=self._build_legends_frame) + omni.ui.Spacer(height=8) + self._limits_frame = omni.ui.Frame(build_fn=self._build_limits_frame) + omni.ui.Spacer(height=8) + self._filter_frame = omni.ui.Frame(build_fn=self._build_filter_frame) + self._is_built = True + + containing_frame = omni.ui.Frame(build_fn=_build_widget) + + return containing_frame + + """ UI Actions Listener Functions """ + + def _change_plot_visibility(self, idx: int, visible: bool): + """Change the visibility of a plot at position idx.""" + self._series_visible[idx] = visible + self._plot_frames[idx].visible = visible + # self._main_plot_frame.rebuild() + + def _set_y_min(self, val: float): + """Update the y-axis minimum.""" + self._y_min = val + self.lower_limit_drag.model.set_value(val) + self._main_plot_frame.rebuild() + + def _set_y_max(self, val: float): + """Update the y-axis maximum.""" + self._y_max = val + self.upper_limit_drag.model.set_value(val) + self._main_plot_frame.rebuild() + + def _rescale_btn_pressed(self): + """Autoscale the y-axis to the current data.""" + if any(self._series_visible): + y_min = np.round( + min([min(y) for idx, y in enumerate(self._y_data) if self._series_visible[idx]]), + 4, + ) + y_max = np.round( + max([max(y) for idx, y in enumerate(self._y_data) if self._series_visible[idx]]), + 4, + ) + if y_min == y_max: + y_max += 1e-4 # Make sure axes don't collapse + + self._y_max = y_max + self._y_min = y_min + + if hasattr(self, "lower_limit_drag") and hasattr(self, "upper_limit_drag"): + self.lower_limit_drag.model.set_value(self._y_min) + self.upper_limit_drag.model.set_value(self._y_max) + + self._main_plot_frame.rebuild() + + """ Helper Functions """ + + def _get_distinct_hex_colors(self, num_colors) -> list[int]: + """ + This function returns a list of distinct colors for plotting. + + Args: + num_colors (int): the number of colors to generate + + Returns: + List[int]: a list of distinct colors in hexadecimal format 0xFFBBGGRR + """ + # Generate equally spaced colors in HSV space + rgb_colors = [ + colorsys.hsv_to_rgb(hue / num_colors, 0.75, 1) for hue in np.linspace(0, num_colors - 1, num_colors) + ] + # Convert to 0-255 RGB values + rgb_colors = [[int(c * 255) for c in rgb] for rgb in rgb_colors] + # Convert to 0xFFBBGGRR format + hex_colors = [0xFF * 16**6 + c[2] * 16**4 + c[1] * 16**2 + c[0] for c in rgb_colors] + return hex_colors diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/manager_live_visualizer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/manager_live_visualizer.py new file mode 100644 index 00000000000..47f480f97e3 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/manager_live_visualizer.py @@ -0,0 +1,302 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy +import weakref +from dataclasses import MISSING +from typing import TYPE_CHECKING + +import omni.kit.app +import omni.log +from isaacsim.core.api.simulation_context import SimulationContext + +from isaaclab.managers import ManagerBase +from isaaclab.utils import configclass + +from .image_plot import ImagePlot +from .line_plot import LiveLinePlot +from .ui_visualizer_base import UiVisualizerBase + +if TYPE_CHECKING: + import omni.ui + + +@configclass +class ManagerLiveVisualizerCfg: + """Configuration for the :class:`ManagerLiveVisualizer` class.""" + + debug_vis: bool = False + """Flag used to set status of the live visualizers on startup. Defaults to False, which means closed.""" + + manager_name: str = MISSING + """Manager name that corresponds to the manager of interest in the ManagerBasedEnv and ManagerBasedRLEnv""" + + term_names: list[str] | dict[str, list[str]] | None = None + """Specific term names specified in a Manager config that are chosen to be plotted. Defaults to None. + + If None all terms will be plotted. For managers that utilize Groups (i.e. ObservationGroup) use a dictionary of + {group_names: [term_names]}. + """ + + +class ManagerLiveVisualizer(UiVisualizerBase): + """A interface object used to transfer data from a manager to a UI widget. + + This class handles the creation of UI Widgets for selected terms given a :class:`ManagerLiveVisualizerCfg`. + It iterates through the terms of the manager and creates a visualizer for each term. If the term is a single + variable or a multi-variable signal, it creates a :class:`LiveLinePlot`. If the term is an image (2D or RGB), + it creates an :class:`ImagePlot`. The visualizer can be toggled on and off using the + :attr:`ManagerLiveVisualizerCfg.debug_vis` flag in the configuration. + """ + + def __init__(self, manager: ManagerBase, cfg: ManagerLiveVisualizerCfg = ManagerLiveVisualizerCfg()): + """Initialize ManagerLiveVisualizer. + + Args: + manager: The manager with terms to be plotted. The manager must have a :meth:`get_active_iterable_terms` method. + cfg: The configuration file used to select desired manager terms to be plotted. + """ + + self._manager = manager + self.debug_vis = cfg.debug_vis + self._env_idx: int = 0 + self.cfg = cfg + self._viewer_env_idx = 0 + self._vis_frame: omni.ui.Frame + self._vis_window: omni.ui.Window + + # evaluate chosen terms if no terms provided use all available. + self.term_names = [] + + if self.cfg.term_names is not None: + # extract chosen terms + if isinstance(self.cfg.term_names, list): + for term_name in self.cfg.term_names: + if term_name in self._manager.active_terms: + self.term_names.append(term_name) + else: + omni.log.error( + f"ManagerVisualizer Failure: ManagerTerm ({term_name}) does not exist in" + f" Manager({self.cfg.manager_name})" + ) + + # extract chosen group-terms + elif isinstance(self.cfg.term_names, dict): + # if manager is using groups and terms are saved as a dictionary + if isinstance(self._manager.active_terms, dict): + for group, terms in self.cfg.term_names: + if group in self._manager.active_terms.keys(): + for term_name in terms: + if term_name in self._manager.active_terms[group]: + self.term_names.append(f"{group}-{term_name}") + else: + omni.log.error( + f"ManagerVisualizer Failure: ManagerTerm ({term_name}) does not exist in" + f" Group({group})" + ) + else: + omni.log.error( + f"ManagerVisualizer Failure: Group ({group}) does not exist in" + f" Manager({self.cfg.manager_name})" + ) + else: + omni.log.error( + f"ManagerVisualizer Failure: Manager({self.cfg.manager_name}) does not utilize grouping of" + " terms." + ) + + # + # Implementation checks + # + + @property + def get_vis_frame(self) -> omni.ui.Frame: + """Returns the UI Frame object tied to this visualizer.""" + return self._vis_frame + + @property + def get_vis_window(self) -> omni.ui.Window: + """Returns the UI Window object tied to this visualizer.""" + return self._vis_window + + # + # Setters + # + + def set_debug_vis(self, debug_vis: bool): + """Set the debug visualization external facing function. + + Args: + debug_vis: Whether to enable or disable the debug visualization. + """ + self._set_debug_vis_impl(debug_vis) + + # + # Implementations + # + + def _set_env_selection_impl(self, env_idx: int): + """Update the index of the selected environment to display. + + Args: + env_idx: The index of the selected environment. + """ + if env_idx > 0 and env_idx < self._manager.num_envs: + self._env_idx = env_idx + else: + omni.log.warn(f"Environment index is out of range (0, {self._manager.num_envs - 1})") + + def _set_vis_frame_impl(self, frame: omni.ui.Frame): + """Updates the assigned frame that can be used for visualizations. + + Args: + frame: The debug visualization frame. + """ + self._vis_frame = frame + + def _debug_vis_callback(self, event): + """Callback for the debug visualization event.""" + + if not SimulationContext.instance().is_playing(): + # Visualizers have not been created yet. + return + + # get updated data and update visualization + for (_, term), vis in zip( + self._manager.get_active_iterable_terms(env_idx=self._env_idx), self._term_visualizers + ): + if isinstance(vis, LiveLinePlot): + vis.add_datapoint(term) + elif isinstance(vis, ImagePlot): + vis.update_image(numpy.array(term)) + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set the debug visualization implementation. + + Args: + debug_vis: Whether to enable or disable debug visualization. + """ + + if not hasattr(self, "_vis_frame"): + raise RuntimeError("No frame set for debug visualization.") + + # Clear internal visualizers + self._term_visualizers = [] + self._vis_frame.clear() + + if debug_vis: + # if enabled create a subscriber for the post update event if it doesn't exist + if not hasattr(self, "_debug_vis_handle") or self._debug_vis_handle is None: + app_interface = omni.kit.app.get_app_interface() + self._debug_vis_handle = app_interface.get_post_update_event_stream().create_subscription_to_pop( + lambda event, obj=weakref.proxy(self): obj._debug_vis_callback(event) + ) + else: + # if disabled remove the subscriber if it exists + if self._debug_vis_handle is not None: + self._debug_vis_handle.unsubscribe() + self._debug_vis_handle = None + + self._vis_frame.visible = False + return + + self._vis_frame.visible = True + + with self._vis_frame: + with omni.ui.VStack(): + # Add a plot in a collapsible frame for each term available + for name, term in self._manager.get_active_iterable_terms(env_idx=self._env_idx): + if name in self.term_names or len(self.term_names) == 0: + frame = omni.ui.CollapsableFrame( + name, + collapsed=False, + style={"border_color": 0xFF8A8777, "padding": 4}, + ) + with frame: + # create line plot for single or multi-variable signals + len_term_shape = len(numpy.array(term).shape) + if len_term_shape <= 2: + plot = LiveLinePlot(y_data=[[elem] for elem in term], plot_height=150, show_legend=True) + self._term_visualizers.append(plot) + # create an image plot for 2d and greater data (i.e. mono and rgb images) + elif len_term_shape == 3: + image = ImagePlot(image=numpy.array(term), label=name) + self._term_visualizers.append(image) + else: + omni.log.warn( + f"ManagerLiveVisualizer: Term ({name}) is not a supported data type for" + " visualization." + ) + frame.collapsed = True + + self._debug_vis = debug_vis + + +@configclass +class DefaultManagerBasedEnvLiveVisCfg: + """Default configuration to use for the ManagerBasedEnv. Each chosen manager assumes all terms will be plotted.""" + + action_live_vis = ManagerLiveVisualizerCfg(manager_name="action_manager") + observation_live_vis = ManagerLiveVisualizerCfg(manager_name="observation_manager") + + +@configclass +class DefaultManagerBasedRLEnvLiveVisCfg(DefaultManagerBasedEnvLiveVisCfg): + """Default configuration to use for the ManagerBasedRLEnv. Each chosen manager assumes all terms will be plotted.""" + + curriculum_live_vis = ManagerLiveVisualizerCfg(manager_name="curriculum_manager") + command_live_vis = ManagerLiveVisualizerCfg(manager_name="command_manager") + reward_live_vis = ManagerLiveVisualizerCfg(manager_name="reward_manager") + termination_live_vis = ManagerLiveVisualizerCfg(manager_name="termination_manager") + + +class EnvLiveVisualizer: + """A class to handle all ManagerLiveVisualizers used in an Environment.""" + + def __init__(self, cfg: object, managers: dict[str, ManagerBase]): + """Initialize the EnvLiveVisualizer. + + Args: + cfg: The configuration file containing terms of ManagerLiveVisualizers. + managers: A dictionary of labeled managers. i.e. {"manager_name",manager}. + """ + self.cfg = cfg + self.managers = managers + self._prepare_terms() + + def _prepare_terms(self): + self._manager_visualizers: dict[str, ManagerLiveVisualizer] = dict() + + # check if config is dict already + if isinstance(self.cfg, dict): + cfg_items = self.cfg.items() + else: + cfg_items = self.cfg.__dict__.items() + + for term_name, term_cfg in cfg_items: + # check if term config is None + if term_cfg is None: + continue + # check if term config is viable + if isinstance(term_cfg, ManagerLiveVisualizerCfg): + # find appropriate manager name + manager = self.managers[term_cfg.manager_name] + self._manager_visualizers[term_cfg.manager_name] = ManagerLiveVisualizer(manager=manager, cfg=term_cfg) + else: + raise TypeError( + f"Provided EnvLiveVisualizer term: '{term_name}' is not of type ManagerLiveVisualizerCfg" + ) + + @property + def manager_visualizers(self) -> dict[str, ManagerLiveVisualizer]: + """A dictionary of labeled ManagerLiveVisualizers associated manager name as key.""" + return self._manager_visualizers diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_visualizer_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_visualizer_base.py new file mode 100644 index 00000000000..26d7f29f98a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_visualizer_base.py @@ -0,0 +1,153 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import inspect +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + import omni.ui + + +class UiVisualizerBase: + """Base Class for components that support debug visualizations that requires access to some UI elements. + + This class provides a set of functions that can be used to assign ui interfaces. + + The following functions are provided: + + * :func:`set_debug_vis`: Assigns a debug visualization interface. This function is called by the main UI + when the checkbox for debug visualization is toggled. + * :func:`set_vis_frame`: Assigns a small frame within the isaac lab tab that can be used to visualize debug + information. Such as e.g. plots or images. It is called by the main UI on startup to create the frame. + * :func:`set_window`: Assigngs the main window that is used by the main UI. This allows the user + to have full controller over all UI elements. But be warned, with great power comes great responsibility. + """ + + """ + Exposed Properties + """ + + @property + def has_debug_vis_implementation(self) -> bool: + """Whether the component has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_debug_vis_impl) + return "NotImplementedError" not in source_code + + @property + def has_vis_frame_implementation(self) -> bool: + """Whether the component has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_vis_frame_impl) + return "NotImplementedError" not in source_code + + @property + def has_window_implementation(self) -> bool: + """Whether the component has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_window_impl) + return "NotImplementedError" not in source_code + + @property + def has_env_selection_implementation(self) -> bool: + """Whether the component has a debug visualization implemented.""" + # check if function raises NotImplementedError + source_code = inspect.getsource(self._set_env_selection_impl) + return "NotImplementedError" not in source_code + + """ + Exposed Setters + """ + + def set_env_selection(self, env_selection: int) -> bool: + """Sets the selected environment id. + + This function is called by the main UI when the user selects a different environment. + + Args: + env_selection: The currently selected environment id. + + Returns: + Whether the environment selection was successfully set. False if the component + does not support environment selection. + """ + # check if environment selection is supported + if not self.has_env_selection_implementation: + return False + # set environment selection + self._set_env_selection_impl(env_selection) + return True + + def set_window(self, window: omni.ui.Window) -> bool: + """Sets the current main ui window. + + This function is called by the main UI when the window is created. It allows the component + to add custom UI elements to the window or to control the window and its elements. + + Args: + window: The ui window. + + Returns: + Whether the window was successfully set. False if the component + does not support this functionality. + """ + # check if window is supported + if not self.has_window_implementation: + return False + # set window + self._set_window_impl(window) + return True + + def set_vis_frame(self, vis_frame: omni.ui.Frame) -> bool: + """Sets the debug visualization frame. + + This function is called by the main UI when the window is created. It allows the component + to modify a small frame within the orbit tab that can be used to visualize debug information. + + Args: + vis_frame: The debug visualization frame. + + Returns: + Whether the debug visualization frame was successfully set. False if the component + does not support debug visualization. + """ + # check if debug visualization is supported + if not self.has_vis_frame_implementation: + return False + # set debug visualization frame + self._set_vis_frame_impl(vis_frame) + return True + + """ + Internal Implementation + """ + + def _set_env_selection_impl(self, env_idx: int): + """Set the environment selection.""" + raise NotImplementedError(f"Environment selection is not implemented for {self.__class__.__name__}.") + + def _set_window_impl(self, window: omni.ui.Window): + """Set the window.""" + raise NotImplementedError(f"Window is not implemented for {self.__class__.__name__}.") + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization state.""" + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") + + def _set_vis_frame_impl(self, vis_frame: omni.ui.Frame): + """Set debug visualization into visualization objects. + + This function is responsible for creating the visualization objects if they don't exist + and input ``debug_vis`` is True. If the visualization objects exist, the function should + set their visibility into the stage. + """ + raise NotImplementedError(f"Debug visualization is not implemented for {self.__class__.__name__}.") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_widget_wrapper.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_widget_wrapper.py new file mode 100644 index 00000000000..1abdfa15de3 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/widgets/ui_widget_wrapper.py @@ -0,0 +1,56 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# This file has been adapted from _isaac_sim/exts/isaacsim.gui.components/isaacsim/gui/components/element_wrappers/base_ui_element_wrappers.py + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import omni + +if TYPE_CHECKING: + import omni.ui + + +class UIWidgetWrapper: + """ + Base class for creating wrappers around any subclass of omni.ui.Widget in order to provide an easy interface + for creating and managing specific types of widgets such as state buttons or file pickers. + """ + + def __init__(self, container_frame: omni.ui.Frame): + self._container_frame = container_frame + + @property + def container_frame(self) -> omni.ui.Frame: + return self._container_frame + + @property + def enabled(self) -> bool: + return self.container_frame.enabled + + @enabled.setter + def enabled(self, value: bool): + self.container_frame.enabled = value + + @property + def visible(self) -> bool: + return self.container_frame.visible + + @visible.setter + def visible(self, value: bool): + self.container_frame.visible = value + + def cleanup(self): + """ + Perform any necessary cleanup + """ + pass diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/__init__.py new file mode 100644 index 00000000000..ec047bb66b1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from .instruction_widget import SimpleTextWidget, show_instruction diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/instruction_widget.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/instruction_widget.py new file mode 100644 index 00000000000..d0baab3bee5 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/ui/xr_widgets/instruction_widget.py @@ -0,0 +1,187 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import asyncio +import functools +import textwrap +from typing import Any, TypeAlias + +import omni.kit.commands +import omni.ui as ui +from isaacsim.core.utils.prims import delete_prim, get_prim_at_path +from omni.kit.xr.scene_view.utils import UiContainer, WidgetComponent +from omni.kit.xr.scene_view.utils.spatial_source import SpatialSource +from pxr import Gf + +Vec3Type: TypeAlias = Gf.Vec3f | Gf.Vec3d + +camera_facing_widget_container = {} +camera_facing_widget_timers = {} + + +class SimpleTextWidget(ui.Widget): + def __init__(self, text: str | None = "Simple Text", style: dict[str, Any] | None = None, **kwargs): + super().__init__(**kwargs) + if style is None: + style = {"font_size": 1, "color": 0xFFFFFFFF} + self._text = text + self._style = style + self._ui_label = None + self._build_ui() + + def set_label_text(self, text: str): + """Update the text displayed by the label.""" + self._text = text + if self._ui_label: + self._ui_label.text = self._text + + def _build_ui(self): + """Build the UI with a window-like rectangle and centered label.""" + with ui.ZStack(): + ui.Rectangle(style={"Rectangle": {"background_color": 0xFF454545, "border_radius": 0.1}}) + with ui.VStack(alignment=ui.Alignment.CENTER): + self._ui_label = ui.Label(self._text, style=self._style, alignment=ui.Alignment.CENTER) + + +def compute_widget_dimensions( + text: str, font_size: float, max_width: float, min_width: float +) -> tuple[float, float, list[str]]: + """ + Estimate widget dimensions based on text content. + + Returns: + actual_width (float): The width, clamped between min_width and max_width. + actual_height (float): The computed height based on wrapped text lines. + lines (List[str]): The list of wrapped text lines. + """ + # Estimate average character width. + char_width = 0.6 * font_size + max_chars_per_line = int(max_width / char_width) + lines = textwrap.wrap(text, width=max_chars_per_line) + if not lines: + lines = [text] + computed_width = max(len(line) for line in lines) * char_width + actual_width = max(min(computed_width, max_width), min_width) + line_height = 1.2 * font_size + actual_height = len(lines) * line_height + return actual_width, actual_height, lines + + +def show_instruction( + text: str, + prim_path_source: str | None = None, + translation: Gf.Vec3d = Gf.Vec3d(0, 0, 0), + display_duration: float | None = 5.0, + max_width: float = 2.5, + min_width: float = 1.0, # Prevent widget from being too narrow. + font_size: float = 0.1, + target_prim_path: str = "/newPrim", +) -> UiContainer | None: + """ + Create and display the instruction widget based on the given text. + + The widget's width and height are computed dynamically based on the input text. + It automatically wraps text that is too long and adjusts the widget's height + accordingly. If a display duration is provided (non-zero), the widget is automatically + hidden after that many seconds. + + Args: + text (str): The instruction text to display. + prim_path_source (Optional[str]): The prim path to be used as a spatial sourcey + for the widget. + translation (Gf.Vec3d): A translation vector specifying the widget's position. + display_duration (Optional[float]): The time in seconds to display the widget before + automatically hiding it. If None or 0, the widget remains visible until manually + hidden. + target_prim_path (str): The target path where the copied prim will be created. + Defaults to "/newPrim". + + Returns: + UiContainer: The container instance holding the instruction widget. + """ + global camera_facing_widget_container, camera_facing_widget_timers + + # Check if widget exists and has different text + if target_prim_path in camera_facing_widget_container: + container, current_text = camera_facing_widget_container[target_prim_path] + if current_text == text: + return container + + # Cancel existing timer if there is one + if target_prim_path in camera_facing_widget_timers: + camera_facing_widget_timers[target_prim_path].cancel() + del camera_facing_widget_timers[target_prim_path] + + container.root.clear() + del camera_facing_widget_container[target_prim_path] + + # Clean up existing widget + if get_prim_at_path(target_prim_path): + delete_prim(target_prim_path) + + # Compute dimensions and wrap text. + width, height, lines = compute_widget_dimensions(text, font_size, max_width, min_width) + wrapped_text = "\n".join(lines) + + # Create the widget component. + widget_component = WidgetComponent( + SimpleTextWidget, + width=width, + height=height, + resolution_scale=300, + widget_args=[wrapped_text, {"font_size": font_size}], + ) + + copied_prim = omni.kit.commands.execute( + "CopyPrim", + path_from=prim_path_source, + path_to=target_prim_path, + exclusive_select=False, + copy_to_introducing_layer=False, + ) + + space_stack = [] + if copied_prim is not None: + space_stack.append(SpatialSource.new_prim_path_source(target_prim_path)) + + space_stack.extend([ + SpatialSource.new_translation_source(translation), + SpatialSource.new_look_at_camera_source(), + ]) + + # Create the UI container with the widget. + container = UiContainer( + widget_component, + space_stack=space_stack, + ) + camera_facing_widget_container[target_prim_path] = (container, text) + + # Schedule auto-hide after the specified display_duration if provided. + if display_duration: + timer = asyncio.get_event_loop().call_later(display_duration, functools.partial(hide, target_prim_path)) + camera_facing_widget_timers[target_prim_path] = timer + + return container + + +def hide(target_prim_path: str = "/newPrim") -> None: + """ + Hide and clean up a specific instruction widget. + Also cleans up associated timer. + """ + global camera_facing_widget_container, camera_facing_widget_timers + + if target_prim_path in camera_facing_widget_container: + container, _ = camera_facing_widget_container[target_prim_path] + container.root.clear() + del camera_facing_widget_container[target_prim_path] + + if target_prim_path in camera_facing_widget_timers: + del camera_facing_widget_timers[target_prim_path] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/__init__.py new file mode 100644 index 00000000000..c245dd09c92 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package containing utilities for common operations and helper functions.""" + +from .array import * +from .buffers import * +from .configclass import configclass +from .dict import * +from .interpolation import * +from .modifiers import * +from .string import * +from .timer import Timer +from .types import * diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/array.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/array.py new file mode 100644 index 00000000000..1e76c0f5adc --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/array.py @@ -0,0 +1,100 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for working with different array backends.""" + +# needed to import for allowing type-hinting: torch.device | str | None +from __future__ import annotations + +import numpy as np +import torch +from typing import Union + +import warp as wp + +TensorData = Union[np.ndarray, torch.Tensor, wp.array] +"""Type definition for a tensor data. + +Union of numpy, torch, and warp arrays. +""" + +TENSOR_TYPES = { + "numpy": np.ndarray, + "torch": torch.Tensor, + "warp": wp.array, +} +"""A dictionary containing the types for each backend. + +The keys are the name of the backend ("numpy", "torch", "warp") and the values are the corresponding type +(``np.ndarray``, ``torch.Tensor``, ``wp.array``). +""" + +TENSOR_TYPE_CONVERSIONS = { + "numpy": {wp.array: lambda x: x.numpy(), torch.Tensor: lambda x: x.detach().cpu().numpy()}, + "torch": {wp.array: lambda x: wp.torch.to_torch(x), np.ndarray: lambda x: torch.from_numpy(x)}, + "warp": {np.array: lambda x: wp.array(x), torch.Tensor: lambda x: wp.torch.from_torch(x)}, +} +"""A nested dictionary containing the conversion functions for each backend. + +The keys of the outer dictionary are the name of target backend ("numpy", "torch", "warp"). The keys of the +inner dictionary are the source backend (``np.ndarray``, ``torch.Tensor``, ``wp.array``). +""" + + +def convert_to_torch( + array: TensorData, + dtype: torch.dtype = None, + device: torch.device | str | None = None, +) -> torch.Tensor: + """Converts a given array into a torch tensor. + + The function tries to convert the array to a torch tensor. If the array is a numpy/warp arrays, or python + list/tuples, it is converted to a torch tensor. If the array is already a torch tensor, it is returned + directly. + + If ``device`` is None, then the function deduces the current device of the data. For numpy arrays, + this defaults to "cpu", for torch tensors it is "cpu" or "cuda", and for warp arrays it is "cuda". + + Note: + Since PyTorch does not support unsigned integer types, unsigned integer arrays are converted to + signed integer arrays. This is done by casting the array to the corresponding signed integer type. + + Args: + array: The input array. It can be a numpy array, warp array, python list/tuple, or torch tensor. + dtype: Target data-type for the tensor. + device: The target device for the tensor. Defaults to None. + + Returns: + The converted array as torch tensor. + """ + # Convert array to tensor + # if the datatype is not currently supported by torch we need to improvise + # supported types are: https://pytorch.org/docs/stable/tensors.html + if isinstance(array, torch.Tensor): + tensor = array + elif isinstance(array, np.ndarray): + if array.dtype == np.uint32: + array = array.astype(np.int32) + # need to deal with object arrays (np.void) separately + tensor = torch.from_numpy(array) + elif isinstance(array, wp.array): + if array.dtype == wp.uint32: + array = array.view(wp.int32) + tensor = wp.to_torch(array) + else: + tensor = torch.Tensor(array) + # Convert tensor to the right device + if device is not None and str(tensor.device) != str(device): + tensor = tensor.to(device) + # Convert dtype of tensor if requested + if dtype is not None and tensor.dtype != dtype: + tensor = tensor.type(dtype) + + return tensor diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/assets.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/assets.py new file mode 100644 index 00000000000..29c6c8a9b97 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/assets.py @@ -0,0 +1,134 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module that defines the host-server where assets and resources are stored. + +By default, we use the Isaac Sim Nucleus Server for hosting assets and resources. This makes +distribution of the assets easier and makes the repository smaller in size code-wise. + +For more information, please check information on `Omniverse Nucleus`_. + +.. _Omniverse Nucleus: https://docs.omniverse.nvidia.com/nucleus/latest/overview/overview.html +""" + +import io +import os +import tempfile +from typing import Literal + +import carb +import omni.client + +NUCLEUS_ASSET_ROOT_DIR = carb.settings.get_settings().get("/persistent/isaac/asset_root/cloud") +"""Path to the root directory on the Nucleus Server.""" + +NVIDIA_NUCLEUS_DIR = f"{NUCLEUS_ASSET_ROOT_DIR}/NVIDIA" +"""Path to the root directory on the NVIDIA Nucleus Server.""" + +ISAAC_NUCLEUS_DIR = f"{NUCLEUS_ASSET_ROOT_DIR}/Isaac" +"""Path to the ``Isaac`` directory on the NVIDIA Nucleus Server.""" + +ISAACLAB_NUCLEUS_DIR = f"{ISAAC_NUCLEUS_DIR}/IsaacLab" +"""Path to the ``Isaac/IsaacLab`` directory on the NVIDIA Nucleus Server.""" + + +def check_file_path(path: str) -> Literal[0, 1, 2]: + """Checks if a file exists on the Nucleus Server or locally. + + Args: + path: The path to the file. + + Returns: + The status of the file. Possible values are listed below. + + * :obj:`0` if the file does not exist + * :obj:`1` if the file exists locally + * :obj:`2` if the file exists on the Nucleus Server + """ + if os.path.isfile(path): + return 1 + # we need to convert backslash to forward slash on Windows for omni.client API + elif omni.client.stat(path.replace(os.sep, "/"))[0] == omni.client.Result.OK: + return 2 + else: + return 0 + + +def retrieve_file_path(path: str, download_dir: str | None = None, force_download: bool = True) -> str: + """Retrieves the path to a file on the Nucleus Server or locally. + + If the file exists locally, then the absolute path to the file is returned. + If the file exists on the Nucleus Server, then the file is downloaded to the local machine + and the absolute path to the file is returned. + + Args: + path: The path to the file. + download_dir: The directory where the file should be downloaded. Defaults to None, in which + case the file is downloaded to the system's temporary directory. + force_download: Whether to force download the file from the Nucleus Server. This will overwrite + the local file if it exists. Defaults to True. + + Returns: + The path to the file on the local machine. + + Raises: + FileNotFoundError: When the file not found locally or on Nucleus Server. + RuntimeError: When the file cannot be copied from the Nucleus Server to the local machine. This + can happen when the file already exists locally and :attr:`force_download` is set to False. + """ + # check file status + file_status = check_file_path(path) + if file_status == 1: + return os.path.abspath(path) + elif file_status == 2: + # resolve download directory + if download_dir is None: + download_dir = tempfile.gettempdir() + else: + download_dir = os.path.abspath(download_dir) + # create download directory if it does not exist + if not os.path.exists(download_dir): + os.makedirs(download_dir) + # download file in temp directory using os + file_name = os.path.basename(omni.client.break_url(path.replace(os.sep, "/")).path) + target_path = os.path.join(download_dir, file_name) + # check if file already exists locally + if not os.path.isfile(target_path) or force_download: + # copy file to local machine + result = omni.client.copy(path.replace(os.sep, "/"), target_path, omni.client.CopyBehavior.OVERWRITE) + if result != omni.client.Result.OK and force_download: + raise RuntimeError(f"Unable to copy file: '{path}'. Is the Nucleus Server running?") + return os.path.abspath(target_path) + else: + raise FileNotFoundError(f"Unable to find the file: {path}") + + +def read_file(path: str) -> io.BytesIO: + """Reads a file from the Nucleus Server or locally. + + Args: + path: The path to the file. + + Raises: + FileNotFoundError: When the file not found locally or on Nucleus Server. + + Returns: + The content of the file. + """ + # check file status + file_status = check_file_path(path) + if file_status == 1: + with open(path, "rb") as f: + return io.BytesIO(f.read()) + elif file_status == 2: + file_content = omni.client.read_file(path.replace(os.sep, "/"))[2] + return io.BytesIO(memoryview(file_content).tobytes()) + else: + raise FileNotFoundError(f"Unable to find the file: {path}") diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/__init__.py new file mode 100644 index 00000000000..ef549fde771 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing different buffers.""" + +from .circular_buffer import CircularBuffer +from .delay_buffer import DelayBuffer +from .timestamped_buffer import TimestampedBuffer diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/circular_buffer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/circular_buffer.py new file mode 100644 index 00000000000..0fb4703f770 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/circular_buffer.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from collections.abc import Sequence + + +class CircularBuffer: + """Circular buffer for storing a history of batched tensor data. + + This class implements a circular buffer for storing a history of batched tensor data. The buffer is + initialized with a maximum length and a batch size. The data is stored in a circular fashion, and the + data can be retrieved in a LIFO (Last-In-First-Out) fashion. The buffer is designed to be used in + multi-environment settings, where each environment has its own data. + + The shape of the appended data is expected to be (batch_size, ...), where the first dimension is the + batch dimension. Correspondingly, the shape of the ring buffer is (max_len, batch_size, ...). + """ + + def __init__(self, max_len: int, batch_size: int, device: str): + """Initialize the circular buffer. + + Args: + max_len: The maximum length of the circular buffer. The minimum allowed value is 1. + batch_size: The batch dimension of the data. + device: The device used for processing. + + Raises: + ValueError: If the buffer size is less than one. + """ + if max_len < 1: + raise ValueError(f"The buffer size should be greater than zero. However, it is set to {max_len}!") + # set the parameters + self._batch_size = batch_size + self._device = device + self._ALL_INDICES = torch.arange(batch_size, device=device) + + # max length tensor for comparisons + self._max_len = torch.full((batch_size,), max_len, dtype=torch.int, device=device) + # number of data pushes passed since the last call to :meth:`reset` + self._num_pushes = torch.zeros(batch_size, dtype=torch.long, device=device) + # the pointer to the current head of the circular buffer (-1 means not initialized) + self._pointer: int = -1 + # the actual buffer for data storage + # note: this is initialized on the first call to :meth:`append` + self._buffer: torch.Tensor = None # type: ignore + + """ + Properties. + """ + + @property + def batch_size(self) -> int: + """The batch size of the ring buffer.""" + return self._batch_size + + @property + def device(self) -> str: + """The device used for processing.""" + return self._device + + @property + def max_length(self) -> int: + """The maximum length of the ring buffer.""" + return int(self._max_len[0].item()) + + @property + def current_length(self) -> torch.Tensor: + """The current length of the buffer. Shape is (batch_size,). + + Since the buffer is circular, the current length is the minimum of the number of pushes + and the maximum length. + """ + return torch.minimum(self._num_pushes, self._max_len) + + @property + def buffer(self) -> torch.Tensor: + """Complete circular buffer with most recent entry at the end and oldest entry at the beginning. + Returns: + Complete circular buffer with most recent entry at the end and oldest entry at the beginning of dimension 1. The shape is [batch_size, max_length, data.shape[1:]]. + """ + buf = self._buffer.clone() + buf = torch.roll(buf, shifts=self.max_length - self._pointer - 1, dims=0) + return torch.transpose(buf, dim0=0, dim1=1) + + """ + Operations. + """ + + def reset(self, batch_ids: Sequence[int] | None = None): + """Reset the circular buffer at the specified batch indices. + + Args: + batch_ids: Elements to reset in the batch dimension. Default is None, which resets all the batch indices. + """ + # resolve all indices + if batch_ids is None: + batch_ids = slice(None) + # reset the number of pushes for the specified batch indices + self._num_pushes[batch_ids] = 0 + if self._buffer is not None: + # set buffer at batch_id reset indices to 0.0 so that the buffer() getter returns the cleared circular buffer after reset. + self._buffer[:, batch_ids, :] = 0.0 + + def append(self, data: torch.Tensor): + """Append the data to the circular buffer. + + Args: + data: The data to append to the circular buffer. The first dimension should be the batch dimension. + Shape is (batch_size, ...). + + Raises: + ValueError: If the input data has a different batch size than the buffer. + """ + # check the batch size + if data.shape[0] != self.batch_size: + raise ValueError(f"The input data has '{data.shape[0]}' batch size while expecting '{self.batch_size}'") + + # move the data to the device + data = data.to(self._device) + # at the first call, initialize the buffer size + if self._buffer is None: + self._pointer = -1 + self._buffer = torch.empty((self.max_length, *data.shape), dtype=data.dtype, device=self._device) + # move the head to the next slot + self._pointer = (self._pointer + 1) % self.max_length + # add the new data to the last layer + self._buffer[self._pointer] = data + # Check for batches with zero pushes and initialize all values in batch to first append + is_first_push = self._num_pushes == 0 + if torch.any(is_first_push): + self._buffer[:, is_first_push] = data[is_first_push] + # increment number of number of pushes for all batches + self._num_pushes += 1 + + def __getitem__(self, key: torch.Tensor) -> torch.Tensor: + """Retrieve the data from the circular buffer in last-in-first-out (LIFO) fashion. + + If the requested index is larger than the number of pushes since the last call to :meth:`reset`, + the oldest stored data is returned. + + Args: + key: The index to retrieve from the circular buffer. The index should be less than the number of pushes + since the last call to :meth:`reset`. Shape is (batch_size,). + + Returns: + The data from the circular buffer. Shape is (batch_size, ...). + + Raises: + ValueError: If the input key has a different batch size than the buffer. + RuntimeError: If the buffer is empty. + """ + # check the batch size + if len(key) != self.batch_size: + raise ValueError(f"The argument 'key' has length {key.shape[0]}, while expecting {self.batch_size}") + # check if the buffer is empty + if torch.any(self._num_pushes == 0) or self._buffer is None: + raise RuntimeError("Attempting to retrieve data on an empty circular buffer. Please append data first.") + + # admissible lag + valid_keys = torch.minimum(key, self._num_pushes - 1) + # the index in the circular buffer (pointer points to the last+1 index) + index_in_buffer = torch.remainder(self._pointer - valid_keys, self.max_length) + # return output + return self._buffer[index_in_buffer, self._ALL_INDICES] diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/delay_buffer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/delay_buffer.py new file mode 100644 index 00000000000..8b050e1179e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/delay_buffer.py @@ -0,0 +1,182 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# needed because we concatenate int and torch.Tensor in the type hints +from __future__ import annotations + +import torch +from collections.abc import Sequence + +from .circular_buffer import CircularBuffer + + +class DelayBuffer: + """Delay buffer that allows retrieving stored data with delays. + + This class uses a batched circular buffer to store input data. Different to a standard circular buffer, + which uses the LIFO (last-in-first-out) principle to retrieve the data, the delay buffer class allows + retrieving data based on the lag set by the user. For instance, if the delay set inside the buffer + is 1, then the second last entry from the stream is retrieved. If it is 2, then the third last entry + and so on. + + The class supports storing a batched tensor data. This means that the shape of the appended data + is expected to be (batch_size, ...), where the first dimension is the batch dimension. Correspondingly, + the delay can be set separately for each batch index. If the requested delay is larger than the current + length of the underlying buffer, the most recent entry is returned. + + .. note:: + By default, the delay buffer has no delay, meaning that the data is returned as is. + """ + + def __init__(self, history_length: int, batch_size: int, device: str): + """Initialize the delay buffer. + + Args: + history_length: The history of the buffer, i.e., the number of time steps in the past that the data + will be buffered. It is recommended to set this value equal to the maximum time-step lag that + is expected. The minimum acceptable value is zero, which means only the latest data is stored. + batch_size: The batch dimension of the data. + device: The device used for processing. + """ + # set the parameters + self._history_length = max(0, history_length) + + # the buffer size: current data plus the history length + self._circular_buffer = CircularBuffer(self._history_length + 1, batch_size, device) + + # the minimum and maximum lags across all batch indices. + self._min_time_lag = 0 + self._max_time_lag = 0 + # the lags for each batch index. + self._time_lags = torch.zeros(batch_size, dtype=torch.int, device=device) + + """ + Properties. + """ + + @property + def batch_size(self) -> int: + """The batch size of the ring buffer.""" + return self._circular_buffer.batch_size + + @property + def device(self) -> str: + """The device used for processing.""" + return self._circular_buffer.device + + @property + def history_length(self) -> int: + """The history length of the delay buffer. + + If zero, only the latest data is stored. If one, the latest and the previous data are stored, and so on. + """ + return self._history_length + + @property + def min_time_lag(self) -> int: + """Minimum amount of time steps that can be delayed. + + This value cannot be negative or larger than :attr:`max_time_lag`. + """ + return self._min_time_lag + + @property + def max_time_lag(self) -> int: + """Maximum amount of time steps that can be delayed. + + This value cannot be greater than :attr:`history_length`. + """ + return self._max_time_lag + + @property + def time_lags(self) -> torch.Tensor: + """The time lag across each batch index. + + The shape of the tensor is (batch_size, ). The value at each index represents the delay for that index. + This value is used to retrieve the data from the buffer. + """ + return self._time_lags + + """ + Operations. + """ + + def set_time_lag(self, time_lag: int | torch.Tensor, batch_ids: Sequence[int] | None = None): + """Sets the time lag for the delay buffer across the provided batch indices. + + Args: + time_lag: The desired delay for the buffer. + + * If an integer is provided, the same delay is set for the provided batch indices. + * If a tensor is provided, the delay is set for each batch index separately. The shape of the tensor + should be (len(batch_ids),). + + batch_ids: The batch indices for which the time lag is set. Default is None, which sets the time lag + for all batch indices. + + Raises: + TypeError: If the type of the :attr:`time_lag` is not int or integer tensor. + ValueError: If the minimum time lag is negative or the maximum time lag is larger than the history length. + """ + # resolve batch indices + if batch_ids is None: + batch_ids = slice(None) + + # parse requested time_lag + if isinstance(time_lag, int): + # set the time lags across provided batch indices + self._time_lags[batch_ids] = time_lag + elif isinstance(time_lag, torch.Tensor): + # check valid dtype for time_lag: must be int or long + if time_lag.dtype not in [torch.int, torch.long]: + raise TypeError(f"Invalid dtype for time_lag: {time_lag.dtype}. Expected torch.int or torch.long.") + # set the time lags + self._time_lags[batch_ids] = time_lag.to(device=self.device) + else: + raise TypeError(f"Invalid type for time_lag: {type(time_lag)}. Expected int or integer tensor.") + + # compute the min and max time lag + self._min_time_lag = int(torch.min(self._time_lags).item()) + self._max_time_lag = int(torch.max(self._time_lags).item()) + # check that time_lag is feasible + if self._min_time_lag < 0: + raise ValueError(f"The minimum time lag cannot be negative. Received: {self._min_time_lag}") + if self._max_time_lag > self._history_length: + raise ValueError( + f"The maximum time lag cannot be larger than the history length. Received: {self._max_time_lag}" + ) + + def reset(self, batch_ids: Sequence[int] | None = None): + """Reset the data in the delay buffer at the specified batch indices. + + Args: + batch_ids: Elements to reset in the batch dimension. Default is None, which resets all the batch indices. + """ + self._circular_buffer.reset(batch_ids) + + def compute(self, data: torch.Tensor) -> torch.Tensor: + """Append the input data to the buffer and returns a stale version of the data based on time lag delay. + + If the requested delay is larger than the number of buffered data points since the last reset, + the function returns the latest data. For instance, if the delay is set to 2 and only one data point + is stored in the buffer, the function will return the latest data. If the delay is set to 2 and three + data points are stored, the function will return the first data point. + + Args: + data: The input data. Shape is (batch_size, ...). + + Returns: + The delayed version of the data from the stored buffer. Shape is (batch_size, ...). + """ + # add the new data to the last layer + self._circular_buffer.append(data) + # return output + delayed_data = self._circular_buffer[self._time_lags] + return delayed_data.clone() diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/timestamped_buffer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/timestamped_buffer.py new file mode 100644 index 00000000000..db5fe015dc9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/buffers/timestamped_buffer.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from dataclasses import dataclass + + +@dataclass +class TimestampedBuffer: + """A buffer class containing data and its timestamp. + + This class is a simple data container that stores a tensor and its timestamp. The timestamp is used to + track the last update of the buffer. The timestamp is set to -1.0 by default, indicating that the buffer + has not been updated yet. The timestamp should be updated whenever the data in the buffer is updated. This + way the buffer can be used to check whether the data is outdated and needs to be refreshed. + + The buffer is useful for creating lazy buffers that only update the data when it is outdated. This can be + useful when the data is expensive to compute or retrieve. For example usage, refer to the data classes in + the :mod:`isaaclab.assets` module. + """ + + data: torch.Tensor = None # type: ignore + """The data stored in the buffer. Default is None, indicating that the buffer is empty.""" + + timestamp: float = -1.0 + """Timestamp at the last update of the buffer. Default is -1.0, indicating that the buffer has not been updated.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/configclass.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/configclass.py new file mode 100644 index 00000000000..4acce6ff7ca --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/configclass.py @@ -0,0 +1,492 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module that provides a wrapper around the Python 3.7 onwards ``dataclasses`` module.""" + +import inspect +import types +from collections.abc import Callable +from copy import deepcopy +from dataclasses import MISSING, Field, dataclass, field, replace +from typing import Any, ClassVar + +from .dict import class_to_dict, update_class_from_dict + +_CONFIGCLASS_METHODS = ["to_dict", "from_dict", "replace", "copy", "validate"] +"""List of class methods added at runtime to dataclass.""" + +""" +Wrapper around dataclass. +""" + + +def __dataclass_transform__(): + """Add annotations decorator for PyLance.""" + return lambda a: a + + +@__dataclass_transform__() +def configclass(cls, **kwargs): + """Wrapper around `dataclass` functionality to add extra checks and utilities. + + As of Python 3.7, the standard dataclasses have two main issues which makes them non-generic for + configuration use-cases. These include: + + 1. Requiring a type annotation for all its members. + 2. Requiring explicit usage of :meth:`field(default_factory=...)` to reinitialize mutable variables. + + This function provides a decorator that wraps around Python's `dataclass`_ utility to deal with + the above two issues. It also provides additional helper functions for dictionary <-> class + conversion and easily copying class instances. + + Usage: + + .. code-block:: python + + from dataclasses import MISSING + + from isaaclab.utils.configclass import configclass + + + @configclass + class ViewerCfg: + eye: list = [7.5, 7.5, 7.5] # field missing on purpose + lookat: list = field(default_factory=[0.0, 0.0, 0.0]) + + + @configclass + class EnvCfg: + num_envs: int = MISSING + episode_length: int = 2000 + viewer: ViewerCfg = ViewerCfg() + + # create configuration instance + env_cfg = EnvCfg(num_envs=24) + + # print information as a dictionary + print(env_cfg.to_dict()) + + # create a copy of the configuration + env_cfg_copy = env_cfg.copy() + + # replace arbitrary fields using keyword arguments + env_cfg_copy = env_cfg_copy.replace(num_envs=32) + + Args: + cls: The class to wrap around. + **kwargs: Additional arguments to pass to :func:`dataclass`. + + Returns: + The wrapped class. + + .. _dataclass: https://docs.python.org/3/library/dataclasses.html + """ + # add type annotations + _add_annotation_types(cls) + # add field factory + _process_mutable_types(cls) + # copy mutable members + # note: we check if user defined __post_init__ function exists and augment it with our own + if hasattr(cls, "__post_init__"): + setattr(cls, "__post_init__", _combined_function(cls.__post_init__, _custom_post_init)) + else: + setattr(cls, "__post_init__", _custom_post_init) + # add helper functions for dictionary conversion + setattr(cls, "to_dict", _class_to_dict) + setattr(cls, "from_dict", _update_class_from_dict) + setattr(cls, "replace", _replace_class_with_kwargs) + setattr(cls, "copy", _copy_class) + setattr(cls, "validate", _validate) + # wrap around dataclass + cls = dataclass(cls, **kwargs) + # return wrapped class + return cls + + +""" +Dictionary <-> Class operations. + +These are redefined here to add new docstrings. +""" + + +def _class_to_dict(obj: object) -> dict[str, Any]: + """Convert an object into dictionary recursively. + + Args: + obj: The object to convert. + + Returns: + Converted dictionary mapping. + """ + return class_to_dict(obj) + + +def _update_class_from_dict(obj, data: dict[str, Any]) -> None: + """Reads a dictionary and sets object variables recursively. + + This function performs in-place update of the class member attributes. + + Args: + obj: The object to update. + data: Input (nested) dictionary to update from. + + Raises: + TypeError: When input is not a dictionary. + ValueError: When dictionary has a value that does not match default config type. + KeyError: When dictionary has a key that does not exist in the default config type. + """ + update_class_from_dict(obj, data, _ns="") + + +def _replace_class_with_kwargs(obj: object, **kwargs) -> object: + """Return a new object replacing specified fields with new values. + + This is especially useful for frozen classes. Example usage: + + .. code-block:: python + + @configclass(frozen=True) + class C: + x: int + y: int + + c = C(1, 2) + c1 = c.replace(x=3) + assert c1.x == 3 and c1.y == 2 + + Args: + obj: The object to replace. + **kwargs: The fields to replace and their new values. + + Returns: + The new object. + """ + return replace(obj, **kwargs) + + +def _copy_class(obj: object) -> object: + """Return a new object with the same fields as the original.""" + return replace(obj) + + +""" +Private helper functions. +""" + + +def _add_annotation_types(cls): + """Add annotations to all elements in the dataclass. + + By definition in Python, a field is defined as a class variable that has a type annotation. + + In case type annotations are not provided, dataclass ignores those members when :func:`__dict__()` is called. + This function adds these annotations to the class variable to prevent any issues in case the user forgets to + specify the type annotation. + + This makes the following a feasible operation: + + @dataclass + class State: + pos = (0.0, 0.0, 0.0) + ^^ + If the function is NOT used, the following type-error is returned: + TypeError: 'pos' is a field but has no type annotation + """ + # get type hints + hints = {} + # iterate over class inheritance + # we add annotations from base classes first + for base in reversed(cls.__mro__): + # check if base is object + if base is object: + continue + # get base class annotations + ann = base.__dict__.get("__annotations__", {}) + # directly add all annotations from base class + hints.update(ann) + # iterate over base class members + # Note: Do not change this to dir(base) since it orders the members alphabetically. + # This is not desirable since the order of the members is important in some cases. + for key in base.__dict__: + # get class member + value = getattr(base, key) + # skip members + if _skippable_class_member(key, value, hints): + continue + # add type annotations for members that don't have explicit type annotations + # for these, we deduce the type from the default value + if not isinstance(value, type): + if key not in hints: + # check if var type is not MISSING + # we cannot deduce type from MISSING! + if value is MISSING: + raise TypeError( + f"Missing type annotation for '{key}' in class '{cls.__name__}'." + " Please add a type annotation or set a default value." + ) + # add type annotation + hints[key] = type(value) + elif key != value.__name__: + # note: we don't want to add type annotations for nested configclass. Thus, we check if + # the name of the type matches the name of the variable. + # since Python 3.10, type hints are stored as strings + hints[key] = f"type[{value.__name__}]" + + # Note: Do not change this line. `cls.__dict__.get("__annotations__", {})` is different from + # `cls.__annotations__` because of inheritance. + cls.__annotations__ = cls.__dict__.get("__annotations__", {}) + cls.__annotations__ = hints + + +def _validate(obj: object, prefix: str = "") -> list[str]: + """Check the validity of configclass object. + + This function checks if the object is a valid configclass object. A valid configclass object contains no MISSING + entries. + + Args: + obj: The object to check. + prefix: The prefix to add to the missing fields. Defaults to ''. + + Returns: + A list of missing fields. + + Raises: + TypeError: When the object is not a valid configuration object. + """ + missing_fields = [] + + if type(obj) is type(MISSING): + missing_fields.append(prefix) + return missing_fields + elif isinstance(obj, (list, tuple)): + for index, item in enumerate(obj): + current_path = f"{prefix}[{index}]" + missing_fields.extend(_validate(item, prefix=current_path)) + return missing_fields + elif isinstance(obj, dict): + obj_dict = obj + elif hasattr(obj, "__dict__"): + obj_dict = obj.__dict__ + else: + return missing_fields + + for key, value in obj_dict.items(): + # disregard builtin attributes + if key.startswith("__"): + continue + current_path = f"{prefix}.{key}" if prefix else key + missing_fields.extend(_validate(value, prefix=current_path)) + + # raise an error only once at the top-level call + if prefix == "" and missing_fields: + formatted_message = "\n".join(f" - {field}" for field in missing_fields) + raise TypeError( + f"Missing values detected in object {obj.__class__.__name__} for the following" + f" fields:\n{formatted_message}\n" + ) + return missing_fields + + +def _process_mutable_types(cls): + """Initialize all mutable elements through :obj:`dataclasses.Field` to avoid unnecessary complaints. + + By default, dataclass requires usage of :obj:`field(default_factory=...)` to reinitialize mutable objects every time a new + class instance is created. If a member has a mutable type and it is created without specifying the `field(default_factory=...)`, + then Python throws an error requiring the usage of `default_factory`. + + Additionally, Python only explicitly checks for field specification when the type is a list, set or dict. This misses the + use-case where the type is class itself. Thus, the code silently carries a bug with it which can lead to undesirable effects. + + This function deals with this issue + + This makes the following a feasible operation: + + @dataclass + class State: + pos: list = [0.0, 0.0, 0.0] + ^^ + If the function is NOT used, the following value-error is returned: + ValueError: mutable default for field pos is not allowed: use default_factory + """ + # note: Need to set this up in the same order as annotations. Otherwise, it + # complains about missing positional arguments. + ann = cls.__dict__.get("__annotations__", {}) + + # iterate over all class members and store them in a dictionary + class_members = {} + for base in reversed(cls.__mro__): + # check if base is object + if base is object: + continue + # iterate over base class members + for key in base.__dict__: + # get class member + f = getattr(base, key) + # skip members + if _skippable_class_member(key, f): + continue + # store class member if it is not a type or if it is already present in annotations + if not isinstance(f, type) or key in ann: + class_members[key] = f + # iterate over base class data fields + # in previous call, things that became a dataclass field were removed from class members + # so we need to add them back here as a dataclass field directly + for key, f in base.__dict__.get("__dataclass_fields__", {}).items(): + # store class member + if not isinstance(f, type): + class_members[key] = f + + # check that all annotations are present in class members + # note: mainly for debugging purposes + if len(class_members) != len(ann): + raise ValueError( + f"In class '{cls.__name__}', number of annotations ({len(ann)}) does not match number of class members" + f" ({len(class_members)}). Please check that all class members have type annotations and/or a default" + " value. If you don't want to specify a default value, please use the literal `dataclasses.MISSING`." + ) + # iterate over annotations and add field factory for mutable types + for key in ann: + # find matching field in class + value = class_members.get(key, MISSING) + # check if key belongs to ClassVar + # in that case, we cannot use default_factory! + origin = getattr(ann[key], "__origin__", None) + if origin is ClassVar: + continue + # check if f is MISSING + # note: commented out for now since it causes issue with inheritance + # of dataclasses when parent have some positional and some keyword arguments. + # Ref: https://stackoverflow.com/questions/51575931/class-inheritance-in-python-3-7-dataclasses + # TODO: check if this is fixed in Python 3.10 + # if f is MISSING: + # continue + if isinstance(value, Field): + setattr(cls, key, value) + elif not isinstance(value, type): + # create field factory for mutable types + value = field(default_factory=_return_f(value)) + setattr(cls, key, value) + + +def _custom_post_init(obj): + """Deepcopy all elements to avoid shared memory issues for mutable objects in dataclasses initialization. + + This function is called explicitly instead of as a part of :func:`_process_mutable_types()` to prevent mapping + proxy type i.e. a read only proxy for mapping objects. The error is thrown when using hierarchical data-classes + for configuration. + """ + for key in dir(obj): + # skip dunder members + if key.startswith("__"): + continue + # get data member + value = getattr(obj, key) + # check annotation + ann = obj.__class__.__dict__.get(key) + # duplicate data members that are mutable + if not callable(value) and not isinstance(ann, property): + setattr(obj, key, deepcopy(value)) + + +def _combined_function(f1: Callable, f2: Callable) -> Callable: + """Combine two functions into one. + + Args: + f1: The first function. + f2: The second function. + + Returns: + The combined function. + """ + + def _combined(*args, **kwargs): + # call both functions + f1(*args, **kwargs) + f2(*args, **kwargs) + + return _combined + + +""" +Helper functions +""" + + +def _skippable_class_member(key: str, value: Any, hints: dict | None = None) -> bool: + """Check if the class member should be skipped in configclass processing. + + The following members are skipped: + + * Dunder members: ``__name__``, ``__module__``, ``__qualname__``, ``__annotations__``, ``__dict__``. + * Manually-added special class functions: From :obj:`_CONFIGCLASS_METHODS`. + * Members that are already present in the type annotations. + * Functions bounded to class object or class. + * Properties bounded to class object. + + Args: + key: The class member name. + value: The class member value. + hints: The type hints for the class. Defaults to None, in which case, the + members existence in type hints are not checked. + + Returns: + True if the class member should be skipped, False otherwise. + """ + # skip dunder members + if key.startswith("__"): + return True + # skip manually-added special class functions + if key in _CONFIGCLASS_METHODS: + return True + # check if key is already present + if hints is not None and key in hints: + return True + # skip functions bounded to class + if callable(value): + # FIXME: This doesn't yet work for static methods because they are essentially seen as function types. + # check for class methods + if isinstance(value, types.MethodType): + return True + # check for instance methods + signature = inspect.signature(value) + if "self" in signature.parameters or "cls" in signature.parameters: + return True + # skip property methods + if isinstance(value, property): + return True + # Otherwise, don't skip + return False + + +def _return_f(f: Any) -> Callable[[], Any]: + """Returns default factory function for creating mutable/immutable variables. + + This function should be used to create default factory functions for variables. + + Example: + + .. code-block:: python + + value = field(default_factory=_return_f(value)) + setattr(cls, key, value) + """ + + def _wrap(): + if isinstance(f, Field): + if f.default_factory is MISSING: + return deepcopy(f.default) + else: + return f.default_factory + else: + return deepcopy(f) + + return _wrap diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/dict.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/dict.py new file mode 100644 index 00000000000..29b59502de1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/dict.py @@ -0,0 +1,313 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for utilities for working with dictionaries.""" + +import collections.abc +import hashlib +import json +import torch +from collections.abc import Iterable, Mapping +from typing import Any + +from .array import TENSOR_TYPE_CONVERSIONS, TENSOR_TYPES +from .string import callable_to_string, string_to_callable, string_to_slice + +""" +Dictionary <-> Class operations. +""" + + +def class_to_dict(obj: object) -> dict[str, Any]: + """Convert an object into dictionary recursively. + + Note: + Ignores all names starting with "__" (i.e. built-in methods). + + Args: + obj: An instance of a class to convert. + + Raises: + ValueError: When input argument is not an object. + + Returns: + Converted dictionary mapping. + """ + # check that input data is class instance + if not hasattr(obj, "__class__"): + raise ValueError(f"Expected a class instance. Received: {type(obj)}.") + # convert object to dictionary + if isinstance(obj, dict): + obj_dict = obj + elif isinstance(obj, torch.Tensor): + # We have to treat torch tensors specially because `torch.tensor.__dict__` returns an empty + # dict, which would mean that a torch.tensor would be stored as an empty dict. Instead we + # want to store it directly as the tensor. + return obj + elif hasattr(obj, "__dict__"): + obj_dict = obj.__dict__ + else: + return obj + + # convert to dictionary + data = dict() + for key, value in obj_dict.items(): + # disregard builtin attributes + if key.startswith("__"): + continue + # check if attribute is callable -- function + if callable(value): + data[key] = callable_to_string(value) + # check if attribute is a dictionary + elif hasattr(value, "__dict__") or isinstance(value, dict): + data[key] = class_to_dict(value) + # check if attribute is a list or tuple + elif isinstance(value, (list, tuple)): + data[key] = type(value)([class_to_dict(v) for v in value]) + else: + data[key] = value + return data + + +def update_class_from_dict(obj, data: dict[str, Any], _ns: str = "") -> None: + """Reads a dictionary and sets object variables recursively. + + This function performs in-place update of the class member attributes. + + Args: + obj: An instance of a class to update. + data: Input dictionary to update from. + _ns: Namespace of the current object. This is useful for nested configuration + classes or dictionaries. Defaults to "". + + Raises: + TypeError: When input is not a dictionary. + ValueError: When dictionary has a value that does not match default config type. + KeyError: When dictionary has a key that does not exist in the default config type. + """ + for key, value in data.items(): + # key_ns is the full namespace of the key + key_ns = _ns + "/" + key + # check if key is present in the object + if hasattr(obj, key) or isinstance(obj, dict): + obj_mem = obj[key] if isinstance(obj, dict) else getattr(obj, key) + if isinstance(value, Mapping): + # recursively call if it is a dictionary + update_class_from_dict(obj_mem, value, _ns=key_ns) + continue + if isinstance(value, Iterable) and not isinstance(value, str): + # check length of value to be safe + if len(obj_mem) != len(value) and obj_mem is not None: + raise ValueError( + f"[Config]: Incorrect length under namespace: {key_ns}." + f" Expected: {len(obj_mem)}, Received: {len(value)}." + ) + if isinstance(obj_mem, tuple): + value = tuple(value) + else: + set_obj = True + # recursively call if iterable contains dictionaries + for i in range(len(obj_mem)): + if isinstance(value[i], dict): + update_class_from_dict(obj_mem[i], value[i], _ns=key_ns) + set_obj = False + # do not set value to obj, otherwise it overwrites the cfg class with the dict + if not set_obj: + continue + elif callable(obj_mem): + # update function name + value = string_to_callable(value) + elif isinstance(value, type(obj_mem)) or value is None: + pass + else: + raise ValueError( + f"[Config]: Incorrect type under namespace: {key_ns}." + f" Expected: {type(obj_mem)}, Received: {type(value)}." + ) + # set value + if isinstance(obj, dict): + obj[key] = value + else: + setattr(obj, key, value) + else: + raise KeyError(f"[Config]: Key not found under namespace: {key_ns}.") + + +""" +Dictionary <-> Hashable operations. +""" + + +def dict_to_md5_hash(data: object) -> str: + """Convert a dictionary into a hashable key using MD5 hash. + + Args: + data: Input dictionary or configuration object to convert. + + Returns: + A string object of double length containing only hexadecimal digits. + """ + # convert to dictionary + if isinstance(data, dict): + encoded_buffer = json.dumps(data, sort_keys=True).encode() + else: + encoded_buffer = json.dumps(class_to_dict(data), sort_keys=True).encode() + # compute hash using MD5 + data_hash = hashlib.md5() + data_hash.update(encoded_buffer) + # return the hash key + return data_hash.hexdigest() + + +""" +Dictionary operations. +""" + + +def convert_dict_to_backend( + data: dict, backend: str = "numpy", array_types: Iterable[str] = ("numpy", "torch", "warp") +) -> dict: + """Convert all arrays or tensors in a dictionary to a given backend. + + This function iterates over the dictionary, converts all arrays or tensors with the given types to + the desired backend, and stores them in a new dictionary. It also works with nested dictionaries. + + Currently supported backends are "numpy", "torch", and "warp". + + Note: + This function only converts arrays or tensors. Other types of data are left unchanged. Mutable types + (e.g. lists) are referenced by the new dictionary, so they are not copied. + + Args: + data: An input dict containing array or tensor data as values. + backend: The backend ("numpy", "torch", "warp") to which arrays in this dict should be converted. + Defaults to "numpy". + array_types: A list containing the types of arrays that should be converted to + the desired backend. Defaults to ("numpy", "torch", "warp"). + + Raises: + ValueError: If the specified ``backend`` or ``array_types`` are unknown, i.e. not in the list of supported + backends ("numpy", "torch", "warp"). + + Returns: + The updated dict with the data converted to the desired backend. + """ + # THINK: Should we also support converting to a specific device, e.g. "cuda:0"? + # Check the backend is valid. + if backend not in TENSOR_TYPE_CONVERSIONS: + raise ValueError(f"Unknown backend '{backend}'. Supported backends are 'numpy', 'torch', and 'warp'.") + # Define the conversion functions for each backend. + tensor_type_conversions = TENSOR_TYPE_CONVERSIONS[backend] + + # Parse the array types and convert them to the corresponding types: "numpy" -> np.ndarray, etc. + parsed_types = list() + for t in array_types: + # Check type is valid. + if t not in TENSOR_TYPES: + raise ValueError(f"Unknown array type: '{t}'. Supported array types are 'numpy', 'torch', and 'warp'.") + # Exclude types that match the backend, since we do not need to convert these. + if t == backend: + continue + # Convert the string types to the corresponding types. + parsed_types.append(TENSOR_TYPES[t]) + + # Convert the data to the desired backend. + output_dict = dict() + for key, value in data.items(): + # Obtain the data type of the current value. + data_type = type(value) + # -- arrays + if data_type in parsed_types: + # check if we have a known conversion. + if data_type not in tensor_type_conversions: + raise ValueError(f"No registered conversion for data type: {data_type} to {backend}!") + # convert the data to the desired backend. + output_dict[key] = tensor_type_conversions[data_type](value) + # -- nested dictionaries + elif isinstance(data[key], dict): + output_dict[key] = convert_dict_to_backend(value) + # -- everything else + else: + output_dict[key] = value + + return output_dict + + +def update_dict(orig_dict: dict, new_dict: collections.abc.Mapping) -> dict: + """Updates existing dictionary with values from a new dictionary. + + This function mimics the dict.update() function. However, it works for + nested dictionaries as well. + + Args: + orig_dict: The original dictionary to insert items to. + new_dict: The new dictionary to insert items from. + + Returns: + The updated dictionary. + """ + for keyname, value in new_dict.items(): + if isinstance(value, collections.abc.Mapping): + orig_dict[keyname] = update_dict(orig_dict.get(keyname, {}), value) + else: + orig_dict[keyname] = value + return orig_dict + + +def replace_slices_with_strings(data: dict) -> dict: + """Replace slice objects with their string representations in a dictionary. + + Args: + data: The dictionary to process. + + Returns: + The dictionary with slice objects replaced by their string representations. + """ + if isinstance(data, dict): + return {k: replace_slices_with_strings(v) for k, v in data.items()} + elif isinstance(data, slice): + return f"slice({data.start},{data.stop},{data.step})" + else: + return data + + +def replace_strings_with_slices(data: dict) -> dict: + """Replace string representations of slices with slice objects in a dictionary. + + Args: + data: The dictionary to process. + + Returns: + The dictionary with string representations of slices replaced by slice objects. + """ + if isinstance(data, dict): + return {k: replace_strings_with_slices(v) for k, v in data.items()} + elif isinstance(data, str) and data.startswith("slice("): + return string_to_slice(data) + else: + return data + + +def print_dict(val, nesting: int = -4, start: bool = True): + """Outputs a nested dictionary.""" + if isinstance(val, dict): + if not start: + print("") + nesting += 4 + for k in val: + print(nesting * " ", end="") + print(k, end=": ") + print_dict(val[k], nesting, start=False) + else: + # deal with functions in print statements + if callable(val): + print(callable_to_string(val)) + else: + print(val) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/__init__.py new file mode 100644 index 00000000000..e95b59c01c1 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodule for different interpolation methods. +""" + +from .linear_interpolation import LinearInterpolation diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/linear_interpolation.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/linear_interpolation.py new file mode 100644 index 00000000000..e86a44abad6 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/interpolation/linear_interpolation.py @@ -0,0 +1,90 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + + +class LinearInterpolation: + """Linearly interpolates a sampled scalar function for arbitrary query points. + + This class implements a linear interpolation for a scalar function. The function maps from real values, x, to + real values, y. It expects a set of samples from the function's domain, x, and the corresponding values, y. + The class allows querying the function's values at any arbitrary point. + + The interpolation is done by finding the two closest points in x to the query point and then linearly + interpolating between the corresponding y values. For the query points that are outside the input points, + the class does a zero-order-hold extrapolation based on the boundary values. This means that the class + returns the value of the closest point in x. + """ + + def __init__(self, x: torch.Tensor, y: torch.Tensor, device: str): + """Initializes the linear interpolation. + + The scalar function maps from real values, x, to real values, y. The input to the class is a set of samples + from the function's domain, x, and the corresponding values, y. + + Note: + The input tensor x should be sorted in ascending order. + + Args: + x: An vector of samples from the function's domain. The values should be sorted in ascending order. + Shape is (num_samples,) + y: The function's values associated to the input x. Shape is (num_samples,) + device: The device used for processing. + + Raises: + ValueError: If the input tensors are empty or have different sizes. + ValueError: If the input tensor x is not sorted in ascending order. + """ + # make sure that input tensors are 1D of size (num_samples,) + self._x = x.view(-1).clone().to(device=device) + self._y = y.view(-1).clone().to(device=device) + + # make sure sizes are correct + if self._x.numel() == 0: + raise ValueError("Input tensor x is empty!") + if self._x.numel() != self._y.numel(): + raise ValueError(f"Input tensors x and y have different sizes: {self._x.numel()} != {self._y.numel()}") + # make sure that x is sorted + if torch.any(self._x[1:] < self._x[:-1]): + raise ValueError("Input tensor x is not sorted in ascending order!") + + def compute(self, q: torch.Tensor) -> torch.Tensor: + """Calculates a linearly interpolated values for the query points. + + Args: + q: The query points. It can have any arbitrary shape. + + Returns: + The interpolated values at query points. It has the same shape as the input tensor. + """ + # serialized q + q_1d = q.view(-1) + # Number of elements in the x that are strictly smaller than query points (use int32 instead of int64) + num_smaller_elements = torch.sum(self._x.unsqueeze(1) < q_1d.unsqueeze(0), dim=0, dtype=torch.int) + + # The index pointing to the first element in x such that x[lower_bound_i] < q_i + # If a point is smaller that all x elements, it will assign 0 + lower_bound = torch.clamp(num_smaller_elements - 1, min=0) + # The index pointing to the first element in x such that x[upper_bound_i] >= q_i + # If a point is greater than all x elements, it will assign the last elements' index + upper_bound = torch.clamp(num_smaller_elements, max=self._x.numel() - 1) + + # compute the weight as: (q_i - x_lb) / (x_ub - x_lb) + weight = (q_1d - self._x[lower_bound]) / (self._x[upper_bound] - self._x[lower_bound]) + # If a point is out of bounds assign weight 0.0 + weight[upper_bound == lower_bound] = 0.0 + + # Perform linear interpolation + fq = self._y[lower_bound] + weight * (self._y[upper_bound] - self._y[lower_bound]) + + # deserialized fq + fq = fq.view(q.shape) + return fq diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/__init__.py new file mode 100644 index 00000000000..d0c7862317f --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Submodules for files IO operations. +""" + +from .pkl import dump_pickle, load_pickle +from .yaml import dump_yaml, load_yaml diff --git a/source/isaaclab/isaaclab/utils/io/pkl.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/pkl.py similarity index 91% rename from source/isaaclab/isaaclab/utils/io/pkl.py rename to install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/pkl.py index dc71fe4630e..cfaf7c64809 100644 --- a/source/isaaclab/isaaclab/utils/io/pkl.py +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/pkl.py @@ -3,6 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + """Utilities for file I/O with pickle.""" import os diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/yaml.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/yaml.py new file mode 100644 index 00000000000..49c1760a811 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/io/yaml.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utilities for file I/O with yaml.""" + +import os +import yaml + +from isaaclab.utils import class_to_dict + + +def load_yaml(filename: str) -> dict: + """Loads an input PKL file safely. + + Args: + filename: The path to pickled file. + + Raises: + FileNotFoundError: When the specified file does not exist. + + Returns: + The data read from the input file. + """ + if not os.path.exists(filename): + raise FileNotFoundError(f"File not found: {filename}") + with open(filename) as f: + data = yaml.full_load(f) + return data + + +def dump_yaml(filename: str, data: dict | object, sort_keys: bool = False): + """Saves data into a YAML file safely. + + Note: + The function creates any missing directory along the file's path. + + Args: + filename: The path to save the file at. + data: The data to save either a dictionary or class object. + sort_keys: Whether to sort the keys in the output file. Defaults to False. + """ + # check ending + if not filename.endswith("yaml"): + filename += ".yaml" + # create directory + if not os.path.exists(os.path.dirname(filename)): + os.makedirs(os.path.dirname(filename), exist_ok=True) + # convert data into dictionary + if not isinstance(data, dict): + data = class_to_dict(data) + # save data + with open(filename, "w") as f: + yaml.dump(data, f, default_flow_style=False, sort_keys=sort_keys) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/math.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/math.py new file mode 100644 index 00000000000..0cd052504ab --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/math.py @@ -0,0 +1,1963 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for various math operations.""" + +# needed to import for allowing type-hinting: torch.Tensor | np.ndarray +from __future__ import annotations + +import math +import numpy as np +import torch +import torch.nn.functional +from typing import Literal + +import omni.log + +""" +General +""" + + +@torch.jit.script +def scale_transform(x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor) -> torch.Tensor: + """Normalizes a given input tensor to a range of [-1, 1]. + + .. note:: + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + Normalized transform of the tensor. Shape is (N, dims). + """ + # default value of center + offset = (lower + upper) * 0.5 + # return normalized tensor + return 2 * (x - offset) / (upper - lower) + + +@torch.jit.script +def unscale_transform(x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor) -> torch.Tensor: + """De-normalizes a given input tensor from range of [-1, 1] to (lower, upper). + + .. note:: + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + De-normalized transform of the tensor. Shape is (N, dims). + """ + # default value of center + offset = (lower + upper) * 0.5 + # return normalized tensor + return x * (upper - lower) * 0.5 + offset + + +@torch.jit.script +def saturate(x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor) -> torch.Tensor: + """Clamps a given input tensor to (lower, upper). + + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + Clamped transform of the tensor. Shape is (N, dims). + """ + return torch.max(torch.min(x, upper), lower) + + +@torch.jit.script +def normalize(x: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: + """Normalizes a given input tensor to unit length. + + Args: + x: Input tensor of shape (N, dims). + eps: A small value to avoid division by zero. Defaults to 1e-9. + + Returns: + Normalized tensor of shape (N, dims). + """ + return x / x.norm(p=2, dim=-1).clamp(min=eps, max=None).unsqueeze(-1) + + +@torch.jit.script +def wrap_to_pi(angles: torch.Tensor) -> torch.Tensor: + r"""Wraps input angles (in radians) to the range :math:`[-\pi, \pi]`. + + This function wraps angles in radians to the range :math:`[-\pi, \pi]`, such that + :math:`\pi` maps to :math:`\pi`, and :math:`-\pi` maps to :math:`-\pi`. In general, + odd positive multiples of :math:`\pi` are mapped to :math:`\pi`, and odd negative + multiples of :math:`\pi` are mapped to :math:`-\pi`. + + The function behaves similar to MATLAB's `wrapToPi `_ + function. + + Args: + angles: Input angles of any shape. + + Returns: + Angles in the range :math:`[-\pi, \pi]`. + """ + # wrap to [0, 2*pi) + wrapped_angle = (angles + torch.pi) % (2 * torch.pi) + # map to [-pi, pi] + # we check for zero in wrapped angle to make it go to pi when input angle is odd multiple of pi + return torch.where((wrapped_angle == 0) & (angles > 0), torch.pi, wrapped_angle - torch.pi) + + +@torch.jit.script +def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: + """Create a new floating-point tensor with the magnitude of input and the sign of other, element-wise. + + Note: + The implementation follows from `torch.copysign`. The function allows a scalar magnitude. + + Args: + mag: The magnitude scalar. + other: The tensor containing values whose signbits are applied to magnitude. + + Returns: + The output tensor. + """ + mag_torch = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0]) + return torch.abs(mag_torch) * torch.sign(other) + + +""" +Rotation +""" + + +@torch.jit.script +def quat_unique(q: torch.Tensor) -> torch.Tensor: + """Convert a unit quaternion to a standard form where the real part is non-negative. + + Quaternion representations have a singularity since ``q`` and ``-q`` represent the same + rotation. This function ensures the real part of the quaternion is non-negative. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + Standardized quaternions. Shape is (..., 4). + """ + return torch.where(q[..., 0:1] < 0, -q, q) + + +@torch.jit.script +def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: + """Convert rotations given as quaternions to rotation matrices. + + Args: + quaternions: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + Rotation matrices. The shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L41-L70 + """ + r, i, j, k = torch.unbind(quaternions, -1) + # pyre-fixme[58]: `/` is not supported for operand types `float` and `Tensor`. + two_s = 2.0 / (quaternions * quaternions).sum(-1) + + o = torch.stack( + ( + 1 - two_s * (j * j + k * k), + two_s * (i * j - k * r), + two_s * (i * k + j * r), + two_s * (i * j + k * r), + 1 - two_s * (i * i + k * k), + two_s * (j * k - i * r), + two_s * (i * k - j * r), + two_s * (j * k + i * r), + 1 - two_s * (i * i + j * j), + ), + -1, + ) + return o.reshape(quaternions.shape[:-1] + (3, 3)) + + +def convert_quat(quat: torch.Tensor | np.ndarray, to: Literal["xyzw", "wxyz"] = "xyzw") -> torch.Tensor | np.ndarray: + """Converts quaternion from one convention to another. + + The convention to convert TO is specified as an optional argument. If to == 'xyzw', + then the input is in 'wxyz' format, and vice-versa. + + Args: + quat: The quaternion of shape (..., 4). + to: Convention to convert quaternion to.. Defaults to "xyzw". + + Returns: + The converted quaternion in specified convention. + + Raises: + ValueError: Invalid input argument `to`, i.e. not "xyzw" or "wxyz". + ValueError: Invalid shape of input `quat`, i.e. not (..., 4,). + """ + # check input is correct + if quat.shape[-1] != 4: + msg = f"Expected input quaternion shape mismatch: {quat.shape} != (..., 4)." + raise ValueError(msg) + if to not in ["xyzw", "wxyz"]: + msg = f"Expected input argument `to` to be 'xyzw' or 'wxyz'. Received: {to}." + raise ValueError(msg) + # check if input is numpy array (we support this backend since some classes use numpy) + if isinstance(quat, np.ndarray): + # use numpy functions + if to == "xyzw": + # wxyz -> xyzw + return np.roll(quat, -1, axis=-1) + else: + # xyzw -> wxyz + return np.roll(quat, 1, axis=-1) + else: + # convert to torch (sanity check) + if not isinstance(quat, torch.Tensor): + quat = torch.tensor(quat, dtype=float) + # convert to specified quaternion type + if to == "xyzw": + # wxyz -> xyzw + return quat.roll(-1, dims=-1) + else: + # xyzw -> wxyz + return quat.roll(1, dims=-1) + + +@torch.jit.script +def quat_conjugate(q: torch.Tensor) -> torch.Tensor: + """Computes the conjugate of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + The conjugate quaternion in (w, x, y, z). Shape is (..., 4). + """ + shape = q.shape + q = q.reshape(-1, 4) + return torch.cat((q[:, 0:1], -q[:, 1:]), dim=-1).view(shape) + + +@torch.jit.script +def quat_inv(q: torch.Tensor) -> torch.Tensor: + """Compute the inverse of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + + Returns: + The inverse quaternion in (w, x, y, z). Shape is (N, 4). + """ + return normalize(quat_conjugate(q)) + + +@torch.jit.script +def quat_from_euler_xyz(roll: torch.Tensor, pitch: torch.Tensor, yaw: torch.Tensor) -> torch.Tensor: + """Convert rotations given as Euler angles in radians to Quaternions. + + Note: + The euler angles are assumed in XYZ convention. + + Args: + roll: Rotation around x-axis (in radians). Shape is (N,). + pitch: Rotation around y-axis (in radians). Shape is (N,). + yaw: Rotation around z-axis (in radians). Shape is (N,). + + Returns: + The quaternion in (w, x, y, z). Shape is (N, 4). + """ + cy = torch.cos(yaw * 0.5) + sy = torch.sin(yaw * 0.5) + cr = torch.cos(roll * 0.5) + sr = torch.sin(roll * 0.5) + cp = torch.cos(pitch * 0.5) + sp = torch.sin(pitch * 0.5) + # compute quaternion + qw = cy * cr * cp + sy * sr * sp + qx = cy * sr * cp - sy * cr * sp + qy = cy * cr * sp + sy * sr * cp + qz = sy * cr * cp - cy * sr * sp + + return torch.stack([qw, qx, qy, qz], dim=-1) + + +@torch.jit.script +def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor: + """Returns torch.sqrt(torch.max(0, x)) but with a zero sub-gradient where x is 0. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L91-L99 + """ + ret = torch.zeros_like(x) + positive_mask = x > 0 + ret[positive_mask] = torch.sqrt(x[positive_mask]) + return ret + + +@torch.jit.script +def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: + """Convert rotations given as rotation matrices to quaternions. + + Args: + matrix: The rotation matrices. Shape is (..., 3, 3). + + Returns: + The quaternion in (w, x, y, z). Shape is (..., 4). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161 + """ + if matrix.size(-1) != 3 or matrix.size(-2) != 3: + raise ValueError(f"Invalid rotation matrix shape {matrix.shape}.") + + batch_dim = matrix.shape[:-2] + m00, m01, m02, m10, m11, m12, m20, m21, m22 = torch.unbind(matrix.reshape(batch_dim + (9,)), dim=-1) + + q_abs = _sqrt_positive_part( + torch.stack( + [ + 1.0 + m00 + m11 + m22, + 1.0 + m00 - m11 - m22, + 1.0 - m00 + m11 - m22, + 1.0 - m00 - m11 + m22, + ], + dim=-1, + ) + ) + + # we produce the desired quaternion multiplied by each of r, i, j, k + quat_by_rijk = torch.stack( + [ + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([q_abs[..., 0] ** 2, m21 - m12, m02 - m20, m10 - m01], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m21 - m12, q_abs[..., 1] ** 2, m10 + m01, m02 + m20], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m02 - m20, m10 + m01, q_abs[..., 2] ** 2, m12 + m21], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m10 - m01, m20 + m02, m21 + m12, q_abs[..., 3] ** 2], dim=-1), + ], + dim=-2, + ) + + # We floor here at 0.1 but the exact level is not important; if q_abs is small, + # the candidate won't be picked. + flr = torch.tensor(0.1).to(dtype=q_abs.dtype, device=q_abs.device) + quat_candidates = quat_by_rijk / (2.0 * q_abs[..., None].max(flr)) + + # if not for numerical problems, quat_candidates[i] should be same (up to a sign), + # forall i; we pick the best-conditioned one (with the largest denominator) + return quat_candidates[torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, :].reshape( + batch_dim + (4,) + ) + + +def _axis_angle_rotation(axis: Literal["X", "Y", "Z"], angle: torch.Tensor) -> torch.Tensor: + """Return the rotation matrices for one of the rotations about an axis of which Euler angles describe, + for each value of the angle given. + + Args: + axis: Axis label "X" or "Y or "Z". + angle: Euler angles in radians of any shape. + + Returns: + Rotation matrices. Shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L164-L191 + """ + cos = torch.cos(angle) + sin = torch.sin(angle) + one = torch.ones_like(angle) + zero = torch.zeros_like(angle) + + if axis == "X": + R_flat = (one, zero, zero, zero, cos, -sin, zero, sin, cos) + elif axis == "Y": + R_flat = (cos, zero, sin, zero, one, zero, -sin, zero, cos) + elif axis == "Z": + R_flat = (cos, -sin, zero, sin, cos, zero, zero, zero, one) + else: + raise ValueError("letter must be either X, Y or Z.") + + return torch.stack(R_flat, -1).reshape(angle.shape + (3, 3)) + + +def matrix_from_euler(euler_angles: torch.Tensor, convention: str) -> torch.Tensor: + """ + Convert rotations given as Euler angles in radians to rotation matrices. + + Args: + euler_angles: Euler angles in radians. Shape is (..., 3). + convention: Convention string of three uppercase letters from {"X", "Y", and "Z"}. + For example, "XYZ" means that the rotations should be applied first about x, + then y, then z. + + Returns: + Rotation matrices. Shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L194-L220 + """ + if euler_angles.dim() == 0 or euler_angles.shape[-1] != 3: + raise ValueError("Invalid input euler angles.") + if len(convention) != 3: + raise ValueError("Convention must have 3 letters.") + if convention[1] in (convention[0], convention[2]): + raise ValueError(f"Invalid convention {convention}.") + for letter in convention: + if letter not in ("X", "Y", "Z"): + raise ValueError(f"Invalid letter {letter} in convention string.") + matrices = [_axis_angle_rotation(c, e) for c, e in zip(convention, torch.unbind(euler_angles, -1))] + # return functools.reduce(torch.matmul, matrices) + return torch.matmul(torch.matmul(matrices[0], matrices[1]), matrices[2]) + + +@torch.jit.script +def euler_xyz_from_quat(quat: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Convert rotations given as quaternions to Euler angles in radians. + + Note: + The euler angles are assumed in XYZ convention. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + + Returns: + A tuple containing roll-pitch-yaw. Each element is a tensor of shape (N,). + + Reference: + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + """ + q_w, q_x, q_y, q_z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3] + # roll (x-axis rotation) + sin_roll = 2.0 * (q_w * q_x + q_y * q_z) + cos_roll = 1 - 2 * (q_x * q_x + q_y * q_y) + roll = torch.atan2(sin_roll, cos_roll) + + # pitch (y-axis rotation) + sin_pitch = 2.0 * (q_w * q_y - q_z * q_x) + pitch = torch.where(torch.abs(sin_pitch) >= 1, copysign(torch.pi / 2.0, sin_pitch), torch.asin(sin_pitch)) + + # yaw (z-axis rotation) + sin_yaw = 2.0 * (q_w * q_z + q_x * q_y) + cos_yaw = 1 - 2 * (q_y * q_y + q_z * q_z) + yaw = torch.atan2(sin_yaw, cos_yaw) + + return roll % (2 * torch.pi), pitch % (2 * torch.pi), yaw % (2 * torch.pi) # TODO: why not wrap_to_pi here ? + + +@torch.jit.script +def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor: + """Convert rotations given as quaternions to axis/angle. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + eps: The tolerance for Taylor approximation. Defaults to 1.0e-6. + + Returns: + Rotations given as a vector in axis angle form. Shape is (..., 3). + The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554 + """ + # Modified to take in quat as [q_w, q_x, q_y, q_z] + # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)] + # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z] + # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta) + # When theta = 0, (sin(theta/2) / theta) is undefined + # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48 + quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0)) + mag = torch.linalg.norm(quat[..., 1:], dim=-1) + half_angle = torch.atan2(mag, quat[..., 0]) + angle = 2.0 * half_angle + # check whether to apply Taylor approximation + sin_half_angles_over_angles = torch.where( + angle.abs() > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48 + ) + return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1) + + +@torch.jit.script +def quat_from_angle_axis(angle: torch.Tensor, axis: torch.Tensor) -> torch.Tensor: + """Convert rotations given as angle-axis to quaternions. + + Args: + angle: The angle turned anti-clockwise in radians around the vector's direction. Shape is (N,). + axis: The axis of rotation. Shape is (N, 3). + + Returns: + The quaternion in (w, x, y, z). Shape is (N, 4). + """ + theta = (angle / 2).unsqueeze(-1) + xyz = normalize(axis) * theta.sin() + w = theta.cos() + return normalize(torch.cat([w, xyz], dim=-1)) + + +@torch.jit.script +def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Multiply two quaternions together. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + The product of the two quaternions in (w, x, y, z). Shape is (..., 4). + + Raises: + ValueError: Input shapes of ``q1`` and ``q2`` are not matching. + """ + # check input is correct + if q1.shape != q2.shape: + msg = f"Expected input quaternion shape mismatch: {q1.shape} != {q2.shape}." + raise ValueError(msg) + # reshape to (N, 4) for multiplication + shape = q1.shape + q1 = q1.reshape(-1, 4) + q2 = q2.reshape(-1, 4) + # extract components from quaternions + w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] + w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] + # perform multiplication + ww = (z1 + x1) * (x2 + y2) + yy = (w1 - y1) * (w2 + z2) + zz = (w1 + y1) * (w2 - z2) + xx = ww + yy + zz + qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) + w = qq - ww + (z1 - y1) * (y2 - z2) + x = qq - xx + (x1 + w1) * (x2 + w2) + y = qq - yy + (w1 - x1) * (y2 + z2) + z = qq - zz + (z1 + y1) * (w2 - x2) + + return torch.stack([w, x, y, z], dim=-1).view(shape) + + +@torch.jit.script +def yaw_quat(quat: torch.Tensor) -> torch.Tensor: + """Extract the yaw component of a quaternion. + + Args: + quat: The orientation in (w, x, y, z). Shape is (..., 4) + + Returns: + A quaternion with only yaw component. + """ + shape = quat.shape + quat_yaw = quat.view(-1, 4) + qw = quat_yaw[:, 0] + qx = quat_yaw[:, 1] + qy = quat_yaw[:, 2] + qz = quat_yaw[:, 3] + yaw = torch.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + quat_yaw = torch.zeros_like(quat_yaw) + quat_yaw[:, 3] = torch.sin(yaw / 2) + quat_yaw[:, 0] = torch.cos(yaw / 2) + quat_yaw = normalize(quat_yaw) + return quat_yaw.view(shape) + + +@torch.jit.script +def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """The box-minus operator (quaternion difference) between two quaternions. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (N, 4). + q2: The second quaternion in (w, x, y, z). Shape is (N, 4). + + Returns: + The difference between the two quaternions. Shape is (N, 3). + + Reference: + https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf + """ + quat_diff = quat_mul(q1, quat_conjugate(q2)) # q1 * q2^-1 + return axis_angle_from_quat(quat_diff) # log(qd) + + +@torch.jit.script +def quat_box_plus(q: torch.Tensor, delta: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor: + """The box-plus operator (quaternion update) to apply an increment to a quaternion. + + Args: + q: The initial quaternion in (w, x, y, z). Shape is (N, 4). + delta: The axis-angle perturbation. Shape is (N, 3). + eps: A small value to avoid division by zero. Defaults to 1e-6. + + Returns: + The updated quaternion after applying the perturbation. Shape is (N, 4). + + Reference: + https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf + """ + delta_norm = torch.clamp_min(torch.linalg.norm(delta, dim=-1, keepdim=True), min=eps) + delta_quat = quat_from_angle_axis(delta_norm.squeeze(-1), delta / delta_norm) # exp(dq) + new_quat = quat_mul(delta_quat, q) # Apply perturbation + return quat_unique(new_quat) + + +@torch.jit.script +def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Apply a quaternion rotation to a vector. + + Args: + quat: The quaternion in (w, x, y, z). Shape is (..., 4). + vec: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + # store shape + shape = vec.shape + # reshape to (N, 3) for multiplication + quat = quat.reshape(-1, 4) + vec = vec.reshape(-1, 3) + # extract components from quaternions + xyz = quat[:, 1:] + t = xyz.cross(vec, dim=-1) * 2 + return (vec + quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) + + +@torch.jit.script +def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Apply an inverse quaternion rotation to a vector. + + Args: + quat: The quaternion in (w, x, y, z). Shape is (..., 4). + vec: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + # store shape + shape = vec.shape + # reshape to (N, 3) for multiplication + quat = quat.reshape(-1, 4) + vec = vec.reshape(-1, 3) + # extract components from quaternions + xyz = quat[:, 1:] + t = xyz.cross(vec, dim=-1) * 2 + return (vec - quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) + + +@torch.jit.script +def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Rotate a vector only around the yaw-direction. + + Args: + quat: The orientation in (w, x, y, z). Shape is (N, 4). + vec: The vector in (x, y, z). Shape is (N, 3). + + Returns: + The rotated vector in (x, y, z). Shape is (N, 3). + """ + quat_yaw = yaw_quat(quat) + return quat_apply(quat_yaw, vec) + + +def quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Rotate a vector by a quaternion along the last dimension of q and v. + .. deprecated v2.1.0: + This function will be removed in a future release in favor of the faster implementation :meth:`quat_apply`. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + # deprecation + omni.log.warn( + "The function 'quat_rotate' will be deprecated in favor of the faster method 'quat_apply'." + " Please use 'quat_apply' instead...." + ) + return quat_apply(q, v) + + +def quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Rotate a vector by the inverse of a quaternion along the last dimension of q and v. + + .. deprecated v2.1.0: + This function will be removed in a future release in favor of the faster implementation :meth:`quat_apply_inverse`. + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + omni.log.warn( + "The function 'quat_rotate_inverse' will be deprecated in favor of the faster method 'quat_apply_inverse'." + " Please use 'quat_apply_inverse' instead...." + ) + return quat_apply_inverse(q, v) + + +@torch.jit.script +def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Computes the rotation difference between two quaternions. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + Angular error between input quaternions in radians. + """ + axis_angle_error = quat_box_minus(q1, q2) + return torch.norm(axis_angle_error, dim=-1) + + +@torch.jit.script +def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor: + """Computes the skew-symmetric matrix of a vector. + + Args: + vec: The input vector. Shape is (3,) or (N, 3). + + Returns: + The skew-symmetric matrix. Shape is (1, 3, 3) or (N, 3, 3). + + Raises: + ValueError: If input tensor is not of shape (..., 3). + """ + # check input is correct + if vec.shape[-1] != 3: + raise ValueError(f"Expected input vector shape mismatch: {vec.shape} != (..., 3).") + # unsqueeze the last dimension + if vec.ndim == 1: + vec = vec.unsqueeze(0) + # create a skew-symmetric matrix + skew_sym_mat = torch.zeros(vec.shape[0], 3, 3, device=vec.device, dtype=vec.dtype) + skew_sym_mat[:, 0, 1] = -vec[:, 2] + skew_sym_mat[:, 0, 2] = vec[:, 1] + skew_sym_mat[:, 1, 2] = -vec[:, 0] + skew_sym_mat[:, 1, 0] = vec[:, 2] + skew_sym_mat[:, 2, 0] = -vec[:, 1] + skew_sym_mat[:, 2, 1] = vec[:, 0] + + return skew_sym_mat + + +""" +Transformations +""" + + +def is_identity_pose(pos: torch.tensor, rot: torch.tensor) -> bool: + """Checks if input poses are identity transforms. + + The function checks if the input position and orientation are close to zero and + identity respectively using L2-norm. It does NOT check the error in the orientation. + + Args: + pos: The cartesian position. Shape is (N, 3). + rot: The quaternion in (w, x, y, z). Shape is (N, 4). + + Returns: + True if all the input poses result in identity transform. Otherwise, False. + """ + # create identity transformations + pos_identity = torch.zeros_like(pos) + rot_identity = torch.zeros_like(rot) + rot_identity[..., 0] = 1 + # compare input to identity + return torch.allclose(pos, pos_identity) and torch.allclose(rot, rot_identity) + + +@torch.jit.script +def combine_frame_transforms( + t01: torch.Tensor, q01: torch.Tensor, t12: torch.Tensor | None = None, q12: torch.Tensor | None = None +) -> tuple[torch.Tensor, torch.Tensor]: + r"""Combine transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{02} = T_{01} \times T_{12}`, + where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. + + Args: + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + t12: Position of frame 2 w.r.t. frame 1. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + q12: Quaternion orientation of frame 2 w.r.t. frame 1 in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + A tuple containing the position and orientation of frame 2 w.r.t. frame 0. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # compute orientation + if q12 is not None: + q02 = quat_mul(q01, q12) + else: + q02 = q01 + # compute translation + if t12 is not None: + t02 = t01 + quat_apply(q01, t12) + else: + t02 = t01 + + return t02, q02 + + +def rigid_body_twist_transform( + v0: torch.Tensor, w0: torch.Tensor, t01: torch.Tensor, q01: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + r"""Transform the linear and angular velocity of a rigid body between reference frames. + + Given the twist of 0 relative to frame 0, this function computes the twist of 1 relative to frame 1 + from the position and orientation of frame 1 relative to frame 0. The transformation follows the + equations: + + .. math:: + + w_11 = R_{10} w_00 = R_{01}^{-1} w_00 + v_11 = R_{10} v_00 + R_{10} (w_00 \times t_01) = R_{01}^{-1} (v_00 + (w_00 \times t_01)) + + where + + - :math:`R_{01}` is the rotation matrix from frame 0 to frame 1 derived from quaternion :math:`q_{01}`. + - :math:`t_{01}` is the position of frame 1 relative to frame 0 expressed in frame 0 + - :math:`w_0` is the angular velocity of 0 in frame 0 + - :math:`v_0` is the linear velocity of 0 in frame 0 + + Args: + v0: Linear velocity of 0 in frame 0. Shape is (N, 3). + w0: Angular velocity of 0 in frame 0. Shape is (N, 3). + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + + Returns: + A tuple containing: + - The transformed linear velocity in frame 1. Shape is (N, 3). + - The transformed angular velocity in frame 1. Shape is (N, 3). + """ + w1 = quat_rotate_inverse(q01, w0) + v1 = quat_rotate_inverse(q01, v0 + torch.cross(w0, t01, dim=-1)) + return v1, w1 + + +# @torch.jit.script +def subtract_frame_transforms( + t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor | None = None, q02: torch.Tensor | None = None +) -> tuple[torch.Tensor, torch.Tensor]: + r"""Subtract transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \times T_{02}`, + where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. + + Args: + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + t02: Position of frame 2 w.r.t. frame 0. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + A tuple containing the position and orientation of frame 2 w.r.t. frame 1. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # compute orientation + q10 = quat_inv(q01) + if q02 is not None: + q12 = quat_mul(q10, q02) + else: + q12 = q10 + # compute translation + if t02 is not None: + t12 = quat_apply(q10, t02 - t01) + else: + t12 = quat_apply(q10, -t01) + return t12, q12 + + +# @torch.jit.script +def compute_pose_error( + t01: torch.Tensor, + q01: torch.Tensor, + t02: torch.Tensor, + q02: torch.Tensor, + rot_error_type: Literal["quat", "axis_angle"] = "axis_angle", +) -> tuple[torch.Tensor, torch.Tensor]: + """Compute the position and orientation error between source and target frames. + + Args: + t01: Position of source frame. Shape is (N, 3). + q01: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4). + t02: Position of target frame. Shape is (N, 3). + q02: Quaternion orientation of target frame in (w, x, y, z). Shape is (N, 4). + rot_error_type: The rotation error type to return: "quat", "axis_angle". + Defaults to "axis_angle". + + Returns: + A tuple containing position and orientation error. Shape of position error is (N, 3). + Shape of orientation error depends on the value of :attr:`rot_error_type`: + + - If :attr:`rot_error_type` is "quat", the orientation error is returned + as a quaternion. Shape is (N, 4). + - If :attr:`rot_error_type` is "axis_angle", the orientation error is + returned as an axis-angle vector. Shape is (N, 3). + + Raises: + ValueError: Invalid rotation error type. + """ + # Compute quaternion error (i.e., difference quaternion) + # Reference: https://personal.utdallas.edu/~sxb027100/dock/quaternion.html + # q_current_norm = q_current * q_current_conj + source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 0] + # q_current_inv = q_current_conj / q_current_norm + source_quat_inv = quat_conjugate(q01) / source_quat_norm.unsqueeze(-1) + # q_error = q_target * q_current_inv + quat_error = quat_mul(q02, source_quat_inv) + + # Compute position error + pos_error = t02 - t01 + + # return error based on specified type + if rot_error_type == "quat": + return pos_error, quat_error + elif rot_error_type == "axis_angle": + # Convert to axis-angle error + axis_angle_error = axis_angle_from_quat(quat_error) + return pos_error, axis_angle_error + else: + raise ValueError(f"Unsupported orientation error type: {rot_error_type}. Valid: 'quat', 'axis_angle'.") + + +@torch.jit.script +def apply_delta_pose( + source_pos: torch.Tensor, source_rot: torch.Tensor, delta_pose: torch.Tensor, eps: float = 1.0e-6 +) -> tuple[torch.Tensor, torch.Tensor]: + """Applies delta pose transformation on source pose. + + The first three elements of `delta_pose` are interpreted as cartesian position displacement. + The remaining three elements of `delta_pose` are interpreted as orientation displacement + in the angle-axis format. + + Args: + source_pos: Position of source frame. Shape is (N, 3). + source_rot: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4).. + delta_pose: Position and orientation displacements. Shape is (N, 6). + eps: The tolerance to consider orientation displacement as zero. Defaults to 1.0e-6. + + Returns: + A tuple containing the displaced position and orientation frames. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # number of poses given + num_poses = source_pos.shape[0] + device = source_pos.device + + # interpret delta_pose[:, 0:3] as target position displacements + target_pos = source_pos + delta_pose[:, 0:3] + # interpret delta_pose[:, 3:6] as target rotation displacements + rot_actions = delta_pose[:, 3:6] + angle = torch.linalg.vector_norm(rot_actions, dim=1) + axis = rot_actions / angle.unsqueeze(-1) + # change from axis-angle to quat convention + identity_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).repeat(num_poses, 1) + rot_delta_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > eps, quat_from_angle_axis(angle, axis), identity_quat + ) + # TODO: Check if this is the correct order for this multiplication. + target_rot = quat_mul(rot_delta_quat, source_rot) + + return target_pos, target_rot + + +# @torch.jit.script +def transform_points( + points: torch.Tensor, pos: torch.Tensor | None = None, quat: torch.Tensor | None = None +) -> torch.Tensor: + r"""Transform input points in a given frame to a target frame. + + This function transform points from a source frame to a target frame. The transformation is defined by the + position :math:`t` and orientation :math:`R` of the target frame in the source frame. + + .. math:: + p_{target} = R_{target} \times p_{source} + t_{target} + + If the input `points` is a batch of points, the inputs `pos` and `quat` must be either a batch of + positions and quaternions or a single position and quaternion. If the inputs `pos` and `quat` are + a single position and quaternion, the same transformation is applied to all points in the batch. + + If either the inputs :attr:`pos` and :attr:`quat` are None, the corresponding transformation is not applied. + + Args: + points: Points to transform. Shape is (N, P, 3) or (P, 3). + pos: Position of the target frame. Shape is (N, 3) or (3,). + Defaults to None, in which case the position is assumed to be zero. + quat: Quaternion orientation of the target frame in (w, x, y, z). Shape is (N, 4) or (4,). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + Transformed points in the target frame. Shape is (N, P, 3) or (P, 3). + + Raises: + ValueError: If the inputs `points` is not of shape (N, P, 3) or (P, 3). + ValueError: If the inputs `pos` is not of shape (N, 3) or (3,). + ValueError: If the inputs `quat` is not of shape (N, 4) or (4,). + """ + points_batch = points.clone() + # check if inputs are batched + is_batched = points_batch.dim() == 3 + # -- check inputs + if points_batch.dim() == 2: + points_batch = points_batch[None] # (P, 3) -> (1, P, 3) + if points_batch.dim() != 3: + raise ValueError(f"Expected points to have dim = 2 or dim = 3: got shape {points.shape}") + if not (pos is None or pos.dim() == 1 or pos.dim() == 2): + raise ValueError(f"Expected pos to have dim = 1 or dim = 2: got shape {pos.shape}") + if not (quat is None or quat.dim() == 1 or quat.dim() == 2): + raise ValueError(f"Expected quat to have dim = 1 or dim = 2: got shape {quat.shape}") + # -- rotation + if quat is not None: + # convert to batched rotation matrix + rot_mat = matrix_from_quat(quat) + if rot_mat.dim() == 2: + rot_mat = rot_mat[None] # (3, 3) -> (1, 3, 3) + # convert points to matching batch size (N, P, 3) -> (N, 3, P) + # and apply rotation + points_batch = torch.matmul(rot_mat, points_batch.transpose_(1, 2)) + # (N, 3, P) -> (N, P, 3) + points_batch = points_batch.transpose_(1, 2) + # -- translation + if pos is not None: + # convert to batched translation vector + if pos.dim() == 1: + pos = pos[None, None, :] # (3,) -> (1, 1, 3) + else: + pos = pos[:, None, :] # (N, 3) -> (N, 1, 3) + # apply translation + points_batch += pos + # -- return points in same shape as input + if not is_batched: + points_batch = points_batch.squeeze(0) # (1, P, 3) -> (P, 3) + + return points_batch + + +""" +Projection operations. +""" + + +@torch.jit.script +def orthogonalize_perspective_depth(depth: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tensor: + """Converts perspective depth image to orthogonal depth image. + + Perspective depth images contain distances measured from the camera's optical center. + Meanwhile, orthogonal depth images provide the distance from the camera's image plane. + This method uses the camera geometry to convert perspective depth to orthogonal depth image. + + The function assumes that the width and height are both greater than 1. + + Args: + depth: The perspective depth images. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). + + Returns: + The orthogonal depth images. Shape matches the input shape of depth images. + + Raises: + ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). + ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). + """ + # Clone inputs to avoid in-place modifications + perspective_depth_batch = depth.clone() + intrinsics_batch = intrinsics.clone() + + # Check if inputs are batched + is_batched = perspective_depth_batch.dim() == 4 or ( + perspective_depth_batch.dim() == 3 and perspective_depth_batch.shape[-1] != 1 + ) + + # Track whether the last dimension was singleton + add_last_dim = False + if perspective_depth_batch.dim() == 4 and perspective_depth_batch.shape[-1] == 1: + add_last_dim = True + perspective_depth_batch = perspective_depth_batch.squeeze(dim=3) # (N, H, W, 1) -> (N, H, W) + if perspective_depth_batch.dim() == 3 and perspective_depth_batch.shape[-1] == 1: + add_last_dim = True + perspective_depth_batch = perspective_depth_batch.squeeze(dim=2) # (H, W, 1) -> (H, W) + + if perspective_depth_batch.dim() == 2: + perspective_depth_batch = perspective_depth_batch[None] # (H, W) -> (1, H, W) + + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + + if is_batched and intrinsics_batch.shape[0] == 1: + intrinsics_batch = intrinsics_batch.expand(perspective_depth_batch.shape[0], -1, -1) # (1, 3, 3) -> (N, 3, 3) + + # Validate input shapes + if perspective_depth_batch.dim() != 3: + raise ValueError(f"Expected depth images to have 2, 3, or 4 dimensions; got {depth.shape}.") + if intrinsics_batch.dim() != 3: + raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3); got {intrinsics.shape}.") + + # Image dimensions + im_height, im_width = perspective_depth_batch.shape[1:] + + # Get the intrinsics parameters + fx = intrinsics_batch[:, 0, 0].view(-1, 1, 1) + fy = intrinsics_batch[:, 1, 1].view(-1, 1, 1) + cx = intrinsics_batch[:, 0, 2].view(-1, 1, 1) + cy = intrinsics_batch[:, 1, 2].view(-1, 1, 1) + + # Create meshgrid of pixel coordinates + u_grid = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + v_grid = torch.arange(im_height, device=depth.device, dtype=depth.dtype) + u_grid, v_grid = torch.meshgrid(u_grid, v_grid, indexing="xy") + + # Expand the grids for batch processing + u_grid = u_grid.unsqueeze(0).expand(perspective_depth_batch.shape[0], -1, -1) + v_grid = v_grid.unsqueeze(0).expand(perspective_depth_batch.shape[0], -1, -1) + + # Compute the squared terms for efficiency + x_term = ((u_grid - cx) / fx) ** 2 + y_term = ((v_grid - cy) / fy) ** 2 + + # Calculate the orthogonal (normal) depth + orthogonal_depth = perspective_depth_batch / torch.sqrt(1 + x_term + y_term) + + # Restore the last dimension if it was present in the input + if add_last_dim: + orthogonal_depth = orthogonal_depth.unsqueeze(-1) + + # Return to original shape if input was not batched + if not is_batched: + orthogonal_depth = orthogonal_depth.squeeze(0) + + return orthogonal_depth + + +@torch.jit.script +def unproject_depth(depth: torch.Tensor, intrinsics: torch.Tensor, is_ortho: bool = True) -> torch.Tensor: + r"""Un-project depth image into a pointcloud. + + This function converts orthogonal or perspective depth images into points given the calibration matrix + of the camera. It uses the following transformation based on camera geometry: + + .. math:: + p_{3D} = K^{-1} \times [u, v, 1]^T \times d + + where :math:`p_{3D}` is the 3D point, :math:`d` is the depth value (measured from the image plane), + :math:`u` and :math:`v` are the pixel coordinates and :math:`K` is the intrinsic matrix. + + The function assumes that the width and height are both greater than 1. This makes the function + deal with many possible shapes of depth images and intrinsics matrices. + + .. note:: + If :attr:`is_ortho` is False, the input depth images are transformed to orthogonal depth images + by using the :meth:`orthogonalize_perspective_depth` method. + + Args: + depth: The depth measurement. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). + is_ortho: Whether the input depth image is orthogonal or perspective depth image. If True, the input + depth image is considered as the *orthogonal* type, where the measurements are from the camera's + image plane. If False, the depth image is considered as the *perspective* type, where the + measurements are from the camera's optical center. Defaults to True. + + Returns: + The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + + Raises: + ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). + ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). + """ + # clone inputs to avoid in-place modifications + intrinsics_batch = intrinsics.clone() + # convert depth image to orthogonal if needed + if not is_ortho: + depth_batch = orthogonalize_perspective_depth(depth, intrinsics) + else: + depth_batch = depth.clone() + + # check if inputs are batched + is_batched = depth_batch.dim() == 4 or (depth_batch.dim() == 3 and depth_batch.shape[-1] != 1) + # make sure inputs are batched + if depth_batch.dim() == 3 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=2) # (H, W, 1) -> (H, W) + if depth_batch.dim() == 2: + depth_batch = depth_batch[None] # (H, W) -> (1, H, W) + if depth_batch.dim() == 4 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=3) # (N, H, W, 1) -> (N, H, W) + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + # check shape of inputs + if depth_batch.dim() != 3: + raise ValueError(f"Expected depth images to have dim = 2 or 3 or 4: got shape {depth.shape}") + if intrinsics_batch.dim() != 3: + raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}") + + # get image height and width + im_height, im_width = depth_batch.shape[1:] + # create image points in homogeneous coordinates (3, H x W) + indices_u = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + indices_v = torch.arange(im_height, device=depth.device, dtype=depth.dtype) + img_indices = torch.stack(torch.meshgrid([indices_u, indices_v], indexing="ij"), dim=0).reshape(2, -1) + pixels = torch.nn.functional.pad(img_indices, (0, 0, 0, 1), mode="constant", value=1.0) + pixels = pixels.unsqueeze(0) # (3, H x W) -> (1, 3, H x W) + + # unproject points into 3D space + points = torch.matmul(torch.inverse(intrinsics_batch), pixels) # (N, 3, H x W) + points = points / points[:, -1, :].unsqueeze(1) # normalize by last coordinate + # flatten depth image (N, H, W) -> (N, H x W) + depth_batch = depth_batch.transpose_(1, 2).reshape(depth_batch.shape[0], -1).unsqueeze(2) + depth_batch = depth_batch.expand(-1, -1, 3) + # scale points by depth + points_xyz = points.transpose_(1, 2) * depth_batch # (N, H x W, 3) + + # return points in same shape as input + if not is_batched: + points_xyz = points_xyz.squeeze(0) + + return points_xyz + + +@torch.jit.script +def project_points(points: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tensor: + r"""Projects 3D points into 2D image plane. + + This project 3D points into a 2D image plane. The transformation is defined by the intrinsic + matrix of the camera. + + .. math:: + + \begin{align} + p &= K \times p_{3D} = \\ + p_{2D} &= \begin{pmatrix} u \\ v \\ d \end{pmatrix} + = \begin{pmatrix} p[0] / p[2] \\ p[1] / p[2] \\ Z \end{pmatrix} + \end{align} + + where :math:`p_{2D} = (u, v, d)` is the projected 3D point, :math:`p_{3D} = (X, Y, Z)` is the + 3D point and :math:`K \in \mathbb{R}^{3 \times 3}` is the intrinsic matrix. + + If `points` is a batch of 3D points and `intrinsics` is a single intrinsic matrix, the same + calibration matrix is applied to all points in the batch. + + Args: + points: The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + intrinsics: Camera's calibration matrix. Shape is (3, 3) or (N, 3, 3). + + Returns: + Projected 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + """ + # clone the inputs to avoid in-place operations modifying the original data + points_batch = points.clone() + intrinsics_batch = intrinsics.clone() + + # check if inputs are batched + is_batched = points_batch.dim() == 2 + # make sure inputs are batched + if points_batch.dim() == 2: + points_batch = points_batch[None] # (P, 3) -> (1, P, 3) + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + # check shape of inputs + if points_batch.dim() != 3: + raise ValueError(f"Expected points to have dim = 3: got shape {points.shape}.") + if intrinsics_batch.dim() != 3: + raise ValueError(f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}.") + + # project points into 2D image plane + points_2d = torch.matmul(intrinsics_batch, points_batch.transpose(1, 2)) + points_2d = points_2d / points_2d[:, -1, :].unsqueeze(1) # normalize by last coordinate + points_2d = points_2d.transpose_(1, 2) # (N, 3, P) -> (N, P, 3) + # replace last coordinate with depth + points_2d[:, :, -1] = points_batch[:, :, -1] + + # return points in same shape as input + if not is_batched: + points_2d = points_2d.squeeze(0) # (1, 3, P) -> (3, P) + + return points_2d + + +""" +Sampling +""" + + +@torch.jit.script +def default_orientation(num: int, device: str) -> torch.Tensor: + """Returns identity rotation transform. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Identity quaternion in (w, x, y, z). Shape is (num, 4). + """ + quat = torch.zeros((num, 4), dtype=torch.float, device=device) + quat[..., 0] = 1.0 + + return quat + + +@torch.jit.script +def random_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation in 3D as quaternion. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Sampled quaternion in (w, x, y, z). Shape is (num, 4). + + Reference: + https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.random.html + """ + # sample random orientation from normal distribution + quat = torch.randn((num, 4), dtype=torch.float, device=device) + # normalize the quaternion + return torch.nn.functional.normalize(quat, p=2.0, dim=-1, eps=1e-12) + + +@torch.jit.script +def random_yaw_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation around z-axis. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Sampled quaternion in (w, x, y, z). Shape is (num, 4). + """ + roll = torch.zeros(num, dtype=torch.float, device=device) + pitch = torch.zeros(num, dtype=torch.float, device=device) + yaw = 2 * torch.pi * torch.rand(num, dtype=torch.float, device=device) + + return quat_from_euler_xyz(roll, pitch, yaw) + + +def sample_triangle(lower: float, upper: float, size: int | tuple[int, ...], device: str) -> torch.Tensor: + """Randomly samples tensor from a triangular distribution. + + Args: + lower: The lower range of the sampled tensor. + upper: The upper range of the sampled tensor. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # create random tensor in the range [-1, 1] + r = 2 * torch.rand(*size, device=device) - 1 + # convert to triangular distribution + r = torch.where(r < 0.0, -torch.sqrt(-r), torch.sqrt(r)) + # rescale back to [0, 1] + r = (r + 1.0) / 2.0 + # rescale to range [lower, upper] + return (upper - lower) * r + lower + + +def sample_uniform( + lower: torch.Tensor | float, upper: torch.Tensor | float, size: int | tuple[int, ...], device: str +) -> torch.Tensor: + """Sample uniformly within a range. + + Args: + lower: Lower bound of uniform range. + upper: Upper bound of uniform range. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # return tensor + return torch.rand(*size, device=device) * (upper - lower) + lower + + +def sample_log_uniform( + lower: torch.Tensor | float, upper: torch.Tensor | float, size: int | tuple[int, ...], device: str +) -> torch.Tensor: + r"""Sample using log-uniform distribution within a range. + + The log-uniform distribution is defined as a uniform distribution in the log-space. It + is useful for sampling values that span several orders of magnitude. The sampled values + are uniformly distributed in the log-space and then exponentiated to get the final values. + + .. math:: + + x = \exp(\text{uniform}(\log(\text{lower}), \log(\text{upper}))) + + Args: + lower: Lower bound of uniform range. + upper: Upper bound of uniform range. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # cast to tensor if not already + if not isinstance(lower, torch.Tensor): + lower = torch.tensor(lower, dtype=torch.float, device=device) + if not isinstance(upper, torch.Tensor): + upper = torch.tensor(upper, dtype=torch.float, device=device) + # sample in log-space and exponentiate + return torch.exp(sample_uniform(torch.log(lower), torch.log(upper), size, device)) + + +def sample_gaussian( + mean: torch.Tensor | float, std: torch.Tensor | float, size: int | tuple[int, ...], device: str +) -> torch.Tensor: + """Sample using gaussian distribution. + + Args: + mean: Mean of the gaussian. + std: Std of the gaussian. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. + """ + if isinstance(mean, float): + if isinstance(size, int): + size = (size,) + return torch.normal(mean=mean, std=std, size=size).to(device=device) + else: + return torch.normal(mean=mean, std=std).to(device=device) + + +def sample_cylinder( + radius: float, h_range: tuple[float, float], size: int | tuple[int, ...], device: str +) -> torch.Tensor: + """Sample 3D points uniformly on a cylinder's surface. + + The cylinder is centered at the origin and aligned with the z-axis. The height of the cylinder is + sampled uniformly from the range :obj:`h_range`, while the radius is fixed to :obj:`radius`. + + The sampled points are returned as a tensor of shape :obj:`(*size, 3)`, i.e. the last dimension + contains the x, y, and z coordinates of the sampled points. + + Args: + radius: The radius of the cylinder. + h_range: The minimum and maximum height of the cylinder. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is :obj:`(*size, 3)`. + """ + # sample angles + angles = (torch.rand(size, device=device) * 2 - 1) * torch.pi + h_min, h_max = h_range + # add shape + if isinstance(size, int): + size = (size, 3) + else: + size += (3,) + # allocate a tensor + xyz = torch.zeros(size, device=device) + xyz[..., 0] = radius * torch.cos(angles) + xyz[..., 1] = radius * torch.sin(angles) + xyz[..., 2].uniform_(h_min, h_max) + # return positions + return xyz + + +""" +Orientation Conversions +""" + + +def convert_camera_frame_orientation_convention( + orientation: torch.Tensor, + origin: Literal["opengl", "ros", "world"] = "opengl", + target: Literal["opengl", "ros", "world"] = "ros", +) -> torch.Tensor: + r"""Converts a quaternion representing a rotation from one convention to another. + + In USD, the camera follows the ``"opengl"`` convention. Thus, it is always in **Y up** convention. + This means that the camera is looking down the -Z axis with the +Y axis pointing up , and +X axis pointing right. + However, in ROS, the camera is looking down the +Z axis with the +Y axis pointing down, and +X axis pointing right. + Thus, the camera needs to be rotated by :math:`180^{\circ}` around the X axis to follow the ROS convention. + + .. math:: + + T_{ROS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD} + + On the other hand, the typical world coordinate system is with +X pointing forward, +Y pointing left, + and +Z pointing up. The camera can also be set in this convention by rotating the camera by :math:`90^{\circ}` + around the X axis and :math:`-90^{\circ}` around the Y axis. + + .. math:: + + T_{WORLD} = \begin{bmatrix} 0 & 0 & -1 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD} + + Thus, based on their application, cameras follow different conventions for their orientation. This function + converts a quaternion from one convention to another. + + Possible conventions are: + + - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention + - :obj:`"ros"` - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention + - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention + + Args: + orientation: Quaternion of form `(w, x, y, z)` with shape (..., 4) in source convention. + origin: Convention to convert from. Defaults to "opengl". + target: Convention to convert to. Defaults to "ros". + + Returns: + Quaternion of form `(w, x, y, z)` with shape (..., 4) in target convention + """ + if target == origin: + return orientation.clone() + + # -- unify input type + if origin == "ros": + # convert from ros to opengl convention + rotm = matrix_from_quat(orientation) + rotm[:, :, 2] = -rotm[:, :, 2] + rotm[:, :, 1] = -rotm[:, :, 1] + # convert to opengl convention + quat_gl = quat_from_matrix(rotm) + elif origin == "world": + # convert from world (x forward and z up) to opengl convention + rotm = matrix_from_quat(orientation) + rotm = torch.matmul( + rotm, + matrix_from_euler(torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ"), + ) + # convert to isaac-sim convention + quat_gl = quat_from_matrix(rotm) + else: + quat_gl = orientation + + # -- convert to target convention + if target == "ros": + # convert from opengl to ros convention + rotm = matrix_from_quat(quat_gl) + rotm[:, :, 2] = -rotm[:, :, 2] + rotm[:, :, 1] = -rotm[:, :, 1] + return quat_from_matrix(rotm) + elif target == "world": + # convert from opengl to world (x forward and z up) convention + rotm = matrix_from_quat(quat_gl) + rotm = torch.matmul( + rotm, + matrix_from_euler(torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ").T, + ) + return quat_from_matrix(rotm) + else: + return quat_gl.clone() + + +def create_rotation_matrix_from_view( + eyes: torch.Tensor, + targets: torch.Tensor, + up_axis: Literal["Y", "Z"] = "Z", + device: str = "cpu", +) -> torch.Tensor: + """Compute the rotation matrix from world to view coordinates. + + This function takes a vector ''eyes'' which specifies the location + of the camera in world coordinates and the vector ''targets'' which + indicate the position of the object. + The output is a rotation matrix representing the transformation + from world coordinates -> view coordinates. + + The inputs eyes and targets can each be a + - 3 element tuple/list + - torch tensor of shape (1, 3) + - torch tensor of shape (N, 3) + + Args: + eyes: Position of the camera in world coordinates. + targets: Position of the object in world coordinates. + up_axis: The up axis of the camera. Defaults to "Z". + device: The device to create torch tensors on. Defaults to "cpu". + + The vectors are broadcast against each other so they all have shape (N, 3). + + Returns: + R: (N, 3, 3) batched rotation matrices + + Reference: + Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685) + """ + if up_axis == "Y": + up_axis_vec = torch.tensor((0, 1, 0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1) + elif up_axis == "Z": + up_axis_vec = torch.tensor((0, 0, 1), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1) + else: + raise ValueError(f"Invalid up axis: {up_axis}. Valid options are 'Y' and 'Z'.") + + # get rotation matrix in opengl format (-Z forward, +Y up) + z_axis = -torch.nn.functional.normalize(targets - eyes, eps=1e-5) + x_axis = torch.nn.functional.normalize(torch.cross(up_axis_vec, z_axis, dim=1), eps=1e-5) + y_axis = torch.nn.functional.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5) + is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True) + if is_close.any(): + replacement = torch.nn.functional.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5) + x_axis = torch.where(is_close, replacement, x_axis) + R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1) + return R.transpose(1, 2) + + +def make_pose(pos: torch.Tensor, rot: torch.Tensor) -> torch.Tensor: + """Creates transformation matrices from positions and rotation matrices. + + Args: + pos: Batch of position vectors with last dimension of 3. + rot: Batch of rotation matrices with last 2 dimensions of (3, 3). + + Returns: + Batch of pose matrices with last 2 dimensions of (4, 4). + """ + assert isinstance(pos, torch.Tensor), "Input must be a torch tensor" + assert isinstance(rot, torch.Tensor), "Input must be a torch tensor" + assert pos.shape[:-1] == rot.shape[:-2] + assert pos.shape[-1] == rot.shape[-2] == rot.shape[-1] == 3 + pose = torch.zeros(pos.shape[:-1] + (4, 4), dtype=pos.dtype, device=pos.device) + pose[..., :3, :3] = rot + pose[..., :3, 3] = pos + pose[..., 3, 3] = 1.0 + return pose + + +def unmake_pose(pose: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Splits transformation matrices into positions and rotation matrices. + + Args: + pose: Batch of pose matrices with last 2 dimensions of (4, 4). + + Returns: + Tuple containing: + - Batch of position vectors with last dimension of 3. + - Batch of rotation matrices with last 2 dimensions of (3, 3). + """ + assert isinstance(pose, torch.Tensor), "Input must be a torch tensor" + return pose[..., :3, 3], pose[..., :3, :3] + + +def pose_inv(pose: torch.Tensor) -> torch.Tensor: + """Computes the inverse of transformation matrices. + + The inverse of a pose matrix [R t; 0 1] is [R.T -R.T*t; 0 1]. + + Args: + pose: Batch of pose matrices with last 2 dimensions of (4, 4). + + Returns: + Batch of inverse pose matrices with last 2 dimensions of (4, 4). + """ + assert isinstance(pose, torch.Tensor), "Input must be a torch tensor" + num_axes = len(pose.shape) + assert num_axes >= 2 + + inv_pose = torch.zeros_like(pose) + + # Take transpose of last 2 dimensions + inv_pose[..., :3, :3] = pose[..., :3, :3].transpose(-1, -2) + + # note: PyTorch matmul wants shapes [..., 3, 3] x [..., 3, 1] -> [..., 3, 1] so we add a dimension and take it away after + inv_pose[..., :3, 3] = torch.matmul(-inv_pose[..., :3, :3], pose[..., :3, 3:4])[..., 0] + inv_pose[..., 3, 3] = 1.0 + return inv_pose + + +def pose_in_A_to_pose_in_B(pose_in_A: torch.Tensor, pose_A_in_B: torch.Tensor) -> torch.Tensor: + """Converts poses from one coordinate frame to another. + + Transforms matrices representing point C in frame A + to matrices representing the same point C in frame B. + + Example usage: + + frame_C_in_B = pose_in_A_to_pose_in_B(frame_C_in_A, frame_A_in_B) + + Args: + pose_in_A: Batch of transformation matrices of point C in frame A. + pose_A_in_B: Batch of transformation matrices of frame A in frame B. + + Returns: + Batch of transformation matrices of point C in frame B. + """ + assert isinstance(pose_in_A, torch.Tensor), "Input must be a torch tensor" + assert isinstance(pose_A_in_B, torch.Tensor), "Input must be a torch tensor" + return torch.matmul(pose_A_in_B, pose_in_A) + + +def quat_slerp(q1: torch.Tensor, q2: torch.Tensor, tau: float) -> torch.Tensor: + """Performs spherical linear interpolation (SLERP) between two quaternions. + + This function does not support batch processing. + + Args: + q1: First quaternion in (w, x, y, z) format. + q2: Second quaternion in (w, x, y, z) format. + tau: Interpolation coefficient between 0 (q1) and 1 (q2). + + Returns: + Interpolated quaternion in (w, x, y, z) format. + """ + assert isinstance(q1, torch.Tensor), "Input must be a torch tensor" + assert isinstance(q2, torch.Tensor), "Input must be a torch tensor" + if tau == 0.0: + return q1 + elif tau == 1.0: + return q2 + d = torch.dot(q1, q2) + if abs(abs(d) - 1.0) < torch.finfo(q1.dtype).eps * 4.0: + return q1 + if d < 0.0: + # Invert rotation + d = -d + q2 *= -1.0 + angle = torch.acos(torch.clamp(d, -1, 1)) + if abs(angle) < torch.finfo(q1.dtype).eps * 4.0: + return q1 + isin = 1.0 / torch.sin(angle) + q1 = q1 * torch.sin((1.0 - tau) * angle) * isin + q2 = q2 * torch.sin(tau * angle) * isin + q1 = q1 + q2 + return q1 + + +def interpolate_rotations(R1: torch.Tensor, R2: torch.Tensor, num_steps: int, axis_angle: bool = True) -> torch.Tensor: + """Interpolates between two rotation matrices. + + Args: + R1: First rotation matrix. (4x4). + R2: Second rotation matrix. (4x4). + num_steps: Number of desired interpolated rotations (excluding start and end). + axis_angle: If True, interpolate in axis-angle representation; + otherwise use slerp. Defaults to True. + + Returns: + Stack of interpolated rotation matrices of shape (num_steps + 1, 4, 4), + including the start and end rotations. + """ + assert isinstance(R1, torch.Tensor), "Input must be a torch tensor" + assert isinstance(R2, torch.Tensor), "Input must be a torch tensor" + if axis_angle: + # Delta rotation expressed as axis-angle + delta_rot_mat = torch.matmul(R2, R1.transpose(-1, -2)) + delta_quat = quat_from_matrix(delta_rot_mat) + delta_axis_angle = axis_angle_from_quat(delta_quat) + + # Grab angle + delta_angle = torch.linalg.norm(delta_axis_angle) + + # Fix the axis, and chunk the angle up into steps + rot_step_size = delta_angle / num_steps + + # Convert into delta rotation matrices, and then convert to absolute rotations + if delta_angle < 0.05: + # Small angle - don't bother with interpolation + rot_steps = torch.stack([R2 for _ in range(num_steps)]) + else: + # Make sure that axis is a unit vector + delta_axis = delta_axis_angle / delta_angle + delta_rot_steps = [ + matrix_from_quat(quat_from_angle_axis(i * rot_step_size, delta_axis)) for i in range(num_steps) + ] + rot_steps = torch.stack([torch.matmul(delta_rot_steps[i], R1) for i in range(num_steps)]) + else: + q1 = quat_from_matrix(R1) + q2 = quat_from_matrix(R2) + rot_steps = torch.stack( + [matrix_from_quat(quat_slerp(q1, q2, tau=float(i) / num_steps)) for i in range(num_steps)] + ) + + # Add in endpoint + rot_steps = torch.cat([rot_steps, R2[None]], dim=0) + + return rot_steps + + +def interpolate_poses( + pose_1: torch.Tensor, + pose_2: torch.Tensor, + num_steps: int = None, + step_size: float = None, + perturb: bool = False, +) -> tuple[torch.Tensor, int]: + """Performs linear interpolation between two poses. + + Args: + pose_1: 4x4 start pose. + pose_2: 4x4 end pose. + num_steps: If provided, specifies the number of desired interpolated points. + Passing 0 corresponds to no interpolation. If None, step_size must be provided. + step_size: If provided, determines number of steps based on distance between poses. + perturb: If True, randomly perturbs interpolated position points. + + Returns: + Tuple containing: + - Array of shape (N + 2, 4, 4) corresponding to the interpolated pose path. + - Number of interpolated points (N) in the path. + """ + assert isinstance(pose_1, torch.Tensor), "Input must be a torch tensor" + assert isinstance(pose_2, torch.Tensor), "Input must be a torch tensor" + assert step_size is None or num_steps is None + + pos1, rot1 = unmake_pose(pose_1) + pos2, rot2 = unmake_pose(pose_2) + + if num_steps == 0: + # Skip interpolation + return ( + torch.cat([pos1[None], pos2[None]], dim=0), + torch.cat([rot1[None], rot2[None]], dim=0), + num_steps, + ) + + delta_pos = pos2 - pos1 + if num_steps is None: + assert torch.norm(delta_pos) > 0 + num_steps = math.ceil(torch.norm(delta_pos) / step_size) + + num_steps += 1 # Include starting pose + assert num_steps >= 2 + + # Linear interpolation of positions + pos_step_size = delta_pos / num_steps + grid = torch.arange(num_steps, dtype=torch.float32) + if perturb: + # Move interpolation grid points by up to half-size forward or backward + perturbations = torch.rand(num_steps - 2) - 0.5 + grid[1:-1] += perturbations + pos_steps = torch.stack([pos1 + grid[i] * pos_step_size for i in range(num_steps)]) + + # Add in endpoint + pos_steps = torch.cat([pos_steps, pos2[None]], dim=0) + + # Interpolate rotations + rot_steps = interpolate_rotations(R1=rot1, R2=rot2, num_steps=num_steps, axis_angle=True) + + pose_steps = make_pose(pos_steps, rot_steps) + return pose_steps, num_steps - 1 + + +def transform_poses_from_frame_A_to_frame_B( + src_poses: torch.Tensor, frame_A: torch.Tensor, frame_B: torch.Tensor +) -> torch.Tensor: + """Transforms poses from one coordinate frame to another preserving relative poses. + + Args: + src_poses: Input pose sequence (shape [T, 4, 4]) from source demonstration. + frame_A: 4x4 frame A pose. + frame_B: 4x4 frame B pose. + + Returns: + Transformed pose sequence of shape [T, 4, 4]. + """ + # Transform source end effector poses to be relative to source object frame + src_poses_rel_frame_B = pose_in_A_to_pose_in_B( + pose_in_A=src_poses, + pose_A_in_B=pose_inv(frame_B[None]), + ) + + # Apply relative poses to current object frame to obtain new target eef poses + transformed_poses = pose_in_A_to_pose_in_B( + pose_in_A=src_poses_rel_frame_B, + pose_A_in_B=frame_A[None], + ) + return transformed_poses + + +def generate_random_rotation(rot_boundary: float = (2 * math.pi)) -> torch.Tensor: + """Generates a random rotation matrix using Euler angles. + + Args: + rot_boundary: Range for random rotation angles around each axis (x, y, z). + + Returns: + 3x3 rotation matrix. + """ + angles = torch.rand(3) * rot_boundary + Rx = torch.tensor( + [[1, 0, 0], [0, torch.cos(angles[0]), -torch.sin(angles[0])], [0, torch.sin(angles[0]), torch.cos(angles[0])]] + ) + + Ry = torch.tensor( + [[torch.cos(angles[1]), 0, torch.sin(angles[1])], [0, 1, 0], [-torch.sin(angles[1]), 0, torch.cos(angles[1])]] + ) + + Rz = torch.tensor( + [[torch.cos(angles[2]), -torch.sin(angles[2]), 0], [torch.sin(angles[2]), torch.cos(angles[2]), 0], [0, 0, 1]] + ) + + # Combined rotation matrix + R = torch.matmul(torch.matmul(Rz, Ry), Rx) + return R + + +def generate_random_translation(pos_boundary: float = 1) -> torch.Tensor: + """Generates a random translation vector. + + Args: + pos_boundary: Range for random translation values in 3D space. + + Returns: + 3-element translation vector. + """ + return torch.rand(3) * 2 * pos_boundary - pos_boundary # Random translation in 3D space + + +def generate_random_transformation_matrix(pos_boundary: float = 1, rot_boundary: float = (2 * math.pi)) -> torch.Tensor: + """Generates a random transformation matrix combining rotation and translation. + + Args: + pos_boundary: Range for random translation values. + rot_boundary: Range for random rotation angles. + + Returns: + 4x4 transformation matrix. + """ + R = generate_random_rotation(rot_boundary) + translation = generate_random_translation(pos_boundary) + + # Create the transformation matrix + T = torch.eye(4) + T[:3, :3] = R + T[:3, 3] = translation + + return T diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/__init__.py new file mode 100644 index 00000000000..b3be2106334 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/__init__.py @@ -0,0 +1,70 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing different modifiers implementations. + +Modifiers are used to apply stateful or stateless modifications to tensor data. They take +in a tensor and a configuration and return a tensor with the modification applied. This way users +can define custom operations to apply to a tensor. For instance, a modifier can be used to normalize +the input data or to apply a rolling average. + +They are primarily used to apply custom operations in the :class:`~isaaclab.managers.ObservationManager` +as an alternative to the built-in noise, clip and scale post-processing operations. For more details, see +the :class:`~isaaclab.managers.ObservationTermCfg` class. + +Usage with a function modifier: + +.. code-block:: python + + import torch + from isaaclab.utils import modifiers + + # create a random tensor + my_tensor = torch.rand(256, 128, device="cuda") + + # create a modifier configuration + cfg = modifiers.ModifierCfg(func=modifiers.clip, params={"bounds": (0.0, torch.inf)}) + + # apply the modifier + my_modified_tensor = cfg.func(my_tensor, cfg) + + +Usage with a class modifier: + +.. code-block:: python + + import torch + from isaaclab.utils import modifiers + + # create a random tensor + my_tensor = torch.rand(256, 128, device="cuda") + + # create a modifier configuration + # a digital filter with a simple delay of 1 timestep + cfg = modifiers.DigitalFilterCfg(A=[0.0], B=[0.0, 1.0]) + + # create the modifier instance + my_modifier = modifiers.DigitalFilter(cfg, my_tensor.shape, "cuda") + + # apply the modifier as a callable object + my_modified_tensor = my_modifier(my_tensor) + +""" + +# isort: off +from .modifier_cfg import ModifierCfg +from .modifier_base import ModifierBase +from .modifier import DigitalFilter +from .modifier_cfg import DigitalFilterCfg +from .modifier import Integrator +from .modifier_cfg import IntegratorCfg + +# isort: on +from .modifier import bias, clip, scale diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier.py new file mode 100644 index 00000000000..c3b6c710623 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier.py @@ -0,0 +1,264 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from .modifier_base import ModifierBase + +if TYPE_CHECKING: + from . import modifier_cfg + +## +# Modifiers as functions +## + + +def scale(data: torch.Tensor, multiplier: float) -> torch.Tensor: + """Scales input data by a multiplier. + + Args: + data: The data to apply the scale to. + multiplier: Value to scale input by. + + Returns: + Scaled data. Shape is the same as data. + """ + return data * multiplier + + +def clip(data: torch.Tensor, bounds: tuple[float | None, float | None]) -> torch.Tensor: + """Clips the data to a minimum and maximum value. + + Args: + data: The data to apply the clip to. + bounds: A tuple containing the minimum and maximum values to clip data to. + If the value is None, that bound is not applied. + + Returns: + Clipped data. Shape is the same as data. + """ + return data.clip(min=bounds[0], max=bounds[1]) + + +def bias(data: torch.Tensor, value: float) -> torch.Tensor: + """Adds a uniform bias to the data. + + Args: + data: The data to add bias to. + value: Value of bias to add to data. + + Returns: + Biased data. Shape is the same as data. + """ + return data + value + + +## +# Sample of class based modifiers +## + + +class DigitalFilter(ModifierBase): + r"""Modifier used to apply digital filtering to the input data. + + `Digital filters `_ are used to process discrete-time + signals to extract useful parts of the signal, such as smoothing, noise reduction, or frequency separation. + + The filter can be implemented as a linear difference equation in the time domain. This equation + can be used to calculate the output at each time-step based on the current and previous inputs and outputs. + + .. math:: + y_{i} = X B - Y A = \sum_{j=0}^{N} b_j x_{i-j} - \sum_{j=1}^{M} a_j y_{i-j} + + where :math:`y_{i}` is the current output of the filter. The array :math:`Y` contains previous + outputs from the filter :math:`\{y_{i-j}\}_{j=1}^M` for :math:`M` previous time-steps. The array + :math:`X` contains current :math:`x_{i}` and previous inputs to the filter + :math:`\{x_{i-j}\}_{j=1}^N` for :math:`N` previous time-steps respectively. + The filter coefficients :math:`A` and :math:`B` are used to design the filter. They are column vectors of + length :math:`M` and :math:`N + 1` respectively. + + Different types of filters can be implemented by choosing different values for :math:`A` and :math:`B`. + We provide some examples below. + + Examples + ^^^^^^^^ + + **Unit Delay Filter** + + A filter that delays the input signal by a single time-step simply outputs the previous input value. + + .. math:: y_{i} = x_{i-1} + + This can be implemented as a digital filter with the coefficients :math:`A = [0.0]` and :math:`B = [0.0, 1.0]`. + + **Moving Average Filter** + + A moving average filter is used to smooth out noise in a signal. It is similar to a low-pass filter + but has a finite impulse response (FIR) and is non-recursive. + + The filter calculates the average of the input signal over a window of time-steps. The linear difference + equation for a moving average filter is: + + .. math:: y_{i} = \frac{1}{N} \sum_{j=0}^{N} x_{i-j} + + This can be implemented as a digital filter with the coefficients :math:`A = [0.0]` and + :math:`B = [1/N, 1/N, \cdots, 1/N]`. + + **First-order recursive low-pass filter** + + A recursive low-pass filter is used to smooth out high-frequency noise in a signal. It is a first-order + infinite impulse response (IIR) filter which means it has a recursive component (previous output) in the + linear difference equation. + + A first-order low-pass IIR filter has the difference equation: + + .. math:: y_{i} = \alpha y_{i-1} + (1-\alpha)x_{i} + + where :math:`\alpha` is a smoothing parameter between 0 and 1. Typically, the value of :math:`\alpha` is + chosen based on the desired cut-off frequency of the filter. + + This filter can be implemented as a digital filter with the coefficients :math:`A = [\alpha]` and + :math:`B = [1 - \alpha]`. + """ + + def __init__(self, cfg: modifier_cfg.DigitalFilterCfg, data_dim: tuple[int, ...], device: str) -> None: + """Initializes digital filter. + + Args: + cfg: Configuration parameters. + data_dim: The dimensions of the data to be modified. First element is the batch size + which usually corresponds to number of environments in the simulation. + device: The device to run the modifier on. + + Raises: + ValueError: If filter coefficients are None. + """ + # check that filter coefficients are not None + if cfg.A is None or cfg.B is None: + raise ValueError("Digital filter coefficients A and B must not be None. Please provide valid coefficients.") + + # initialize parent class + super().__init__(cfg, data_dim, device) + + # assign filter coefficients and make sure they are column vectors + self.A = torch.tensor(self._cfg.A, device=self._device).unsqueeze(1) + self.B = torch.tensor(self._cfg.B, device=self._device).unsqueeze(1) + + # create buffer for input and output history + self.x_n = torch.zeros(self._data_dim + (self.B.shape[0],), device=self._device) + self.y_n = torch.zeros(self._data_dim + (self.A.shape[0],), device=self._device) + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets digital filter history. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + """ + if env_ids is None: + env_ids = slice(None) + # reset history buffers + self.x_n[env_ids] = 0.0 + self.y_n[env_ids] = 0.0 + + def __call__(self, data: torch.Tensor) -> torch.Tensor: + """Applies digital filter modification with a rolling history window inputs and outputs. + + Args: + data: The data to apply filter to. + + Returns: + Filtered data. Shape is the same as data. + """ + # move history window for input + self.x_n = torch.roll(self.x_n, shifts=1, dims=-1) + self.x_n[..., 0] = data + + # calculate current filter value: y[i] = Y*A - X*B + y_i = torch.matmul(self.x_n, self.B) - torch.matmul(self.y_n, self.A) + y_i.squeeze_(-1) + + # move history window for output and add current filter value to history + self.y_n = torch.roll(self.y_n, shifts=1, dims=-1) + self.y_n[..., 0] = y_i + + return y_i + + +class Integrator(ModifierBase): + r"""Modifier that applies a numerical forward integration based on a middle Reimann sum. + + An integrator is used to calculate the integral of a signal over time. The integral of a signal + is the area under the curve of the signal. The integral can be approximated using numerical methods + such as the `Riemann sum `_. + + The middle Riemann sum is a method to approximate the integral of a function by dividing the area + under the curve into rectangles. The height of each rectangle is the value of the function at the + midpoint of the interval. The area of each rectangle is the width of the interval multiplied by the + height of the rectangle. + + This integral method is useful for signals that are sampled at regular intervals. The integral + can be written as: + + .. math:: + \int_{t_0}^{t_n} f(t) dt & \approx \int_{t_0}^{t_{n-1}} f(t) dt + \frac{f(t_{n-1}) + f(t_n)}{2} \Delta t + + where :math:`f(t)` is the signal to integrate, :math:`t_i` is the time at the i-th sample, and + :math:`\Delta t` is the time step between samples. + """ + + def __init__(self, cfg: modifier_cfg.IntegratorCfg, data_dim: tuple[int, ...], device: str): + """Initializes the integrator configuration and state. + + Args: + cfg: Integral parameters. + data_dim: The dimensions of the data to be modified. First element is the batch size + which usually corresponds to number of environments in the simulation. + device: The device to run the modifier on. + """ + # initialize parent class + super().__init__(cfg, data_dim, device) + + # assign buffer for integral and previous value + self.integral = torch.zeros(self._data_dim, device=self._device) + self.y_prev = torch.zeros(self._data_dim, device=self._device) + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets integrator state to zero. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + """ + if env_ids is None: + env_ids = slice(None) + # reset history buffers + self.integral[env_ids] = 0.0 + self.y_prev[env_ids] = 0.0 + + def __call__(self, data: torch.Tensor) -> torch.Tensor: + """Applies integral modification to input data. + + Args: + data: The data to integrate. + + Returns: + Integral of input signal. Shape is the same as data. + """ + # integrate using middle Riemann sum + self.integral += (data + self.y_prev) / 2 * self._cfg.dt + # update previous value + self.y_prev[:] = data + + return self.integral diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_base.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_base.py new file mode 100644 index 00000000000..f6e9c0bbe6e --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_base.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from abc import ABC, abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from .modifier_cfg import ModifierCfg + + +class ModifierBase(ABC): + """Base class for modifiers implemented as classes. + + Modifiers implementations can be functions or classes. If a modifier is a class, it should + inherit from this class and implement the required methods. + + A class implementation of a modifier can be used to store state information between calls. + This is useful for modifiers that require stateful operations, such as rolling averages + or delays or decaying filters. + + Example pseudo-code to create and use the class: + + .. code-block:: python + + from isaaclab.utils import modifiers + + # define custom keyword arguments to pass to ModifierCfg + kwarg_dict = {"arg_1" : VAL_1, "arg_2" : VAL_2} + + # create modifier configuration object + # func is the class name of the modifier and params is the dictionary of arguments + modifier_config = modifiers.ModifierCfg(func=modifiers.ModifierBase, params=kwarg_dict) + + # define modifier instance + my_modifier = modifiers.ModifierBase(cfg=modifier_config) + + """ + + def __init__(self, cfg: ModifierCfg, data_dim: tuple[int, ...], device: str) -> None: + """Initializes the modifier class. + + Args: + cfg: Configuration parameters. + data_dim: The dimensions of the data to be modified. First element is the batch size + which usually corresponds to number of environments in the simulation. + device: The device to run the modifier on. + """ + self._cfg = cfg + self._data_dim = data_dim + self._device = device + + @abstractmethod + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the Modifier. + + Args: + env_ids: The environment ids. Defaults to None, in which case + all environments are considered. + """ + raise NotImplementedError + + @abstractmethod + def __call__(self, data: torch.Tensor) -> torch.Tensor: + """Abstract method for defining the modification function. + + Args: + data: The data to be modified. Shape should match the data_dim passed during initialization. + + Returns: + Modified data. Shape is the same as the input data. + """ + raise NotImplementedError diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_cfg.py new file mode 100644 index 00000000000..c8db5d61de9 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/modifiers/modifier_cfg.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch +from collections.abc import Callable +from dataclasses import MISSING +from typing import Any + +from isaaclab.utils import configclass + +from . import modifier + + +@configclass +class ModifierCfg: + """Configuration parameters modifiers""" + + func: Callable[..., torch.Tensor] = MISSING + """Function or callable class used by modifier. + + The function must take a torch tensor as the first argument. The remaining arguments are specified + in the :attr:`params` attribute. + + It also supports `callable classes `_, + i.e. classes that implement the ``__call__()`` method. In this case, the class should inherit from the + :class:`ModifierBase` class and implement the required methods. + """ + + params: dict[str, Any] = dict() + """The parameters to be passed to the function or callable class as keyword arguments. Defaults to + an empty dictionary.""" + + +@configclass +class DigitalFilterCfg(ModifierCfg): + """Configuration parameters for a digital filter modifier. + + For more information, please check the :class:`DigitalFilter` class. + """ + + func: type[modifier.DigitalFilter] = modifier.DigitalFilter + """The digital filter function to be called for applying the filter.""" + + A: list[float] = MISSING + """The coefficients corresponding the the filter's response to past outputs. + + These correspond to the weights of the past outputs of the filter. The first element is the coefficient + for the output at the previous time step, the second element is the coefficient for the output at two + time steps ago, and so on. + + It is the denominator coefficients of the transfer function of the filter. + """ + + B: list[float] = MISSING + """The coefficients corresponding the the filter's response to current and past inputs. + + These correspond to the weights of the current and past inputs of the filter. The first element is the + coefficient for the current input, the second element is the coefficient for the input at the previous + time step, and so on. + + It is the numerator coefficients of the transfer function of the filter. + """ + + +@configclass +class IntegratorCfg(ModifierCfg): + """Configuration parameters for an integrator modifier. + + For more information, please check the :class:`Integrator` class. + """ + + func: type[modifier.Integrator] = modifier.Integrator + """The integrator function to be called for applying the integrator.""" + + dt: float = MISSING + """The time step of the integrator.""" diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/__init__.py new file mode 100644 index 00000000000..b8af62c6f3c --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/__init__.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing different noise models implementations. + +The noise models are implemented as functions that take in a tensor and a configuration and return a tensor +with the noise applied. These functions are then used in the :class:`NoiseCfg` configuration class. + +Usage: + +.. code-block:: python + + import torch + from isaaclab.utils.noise import AdditiveGaussianNoiseCfg + + # create a random tensor + my_tensor = torch.rand(128, 128, device="cuda") + + # create a noise configuration + cfg = AdditiveGaussianNoiseCfg(mean=0.0, std=1.0) + + # apply the noise + my_noisified_tensor = cfg.func(my_tensor, cfg) + +""" +from .noise_cfg import NoiseCfg # noqa: F401 +from .noise_cfg import ConstantNoiseCfg, GaussianNoiseCfg, NoiseModelCfg, NoiseModelWithAdditiveBiasCfg, UniformNoiseCfg +from .noise_model import NoiseModel, NoiseModelWithAdditiveBias, constant_noise, gaussian_noise, uniform_noise + +# Backward compatibility +ConstantBiasNoiseCfg = ConstantNoiseCfg +AdditiveUniformNoiseCfg = UniformNoiseCfg +AdditiveGaussianNoiseCfg = GaussianNoiseCfg diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_cfg.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_cfg.py new file mode 100644 index 00000000000..669fe09c730 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_cfg.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Callable +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from . import noise_model + + +@configclass +class NoiseCfg: + """Base configuration for a noise term.""" + + func: Callable[[torch.Tensor, NoiseCfg], torch.Tensor] = MISSING + """The function to be called for applying the noise. + + Note: + The shape of the input and output tensors must be the same. + """ + operation: Literal["add", "scale", "abs"] = "add" + """The operation to apply the noise on the data. Defaults to "add".""" + + +@configclass +class ConstantNoiseCfg(NoiseCfg): + """Configuration for an additive constant noise term.""" + + func = noise_model.constant_noise + + bias: torch.Tensor | float = 0.0 + """The bias to add. Defaults to 0.0.""" + + +@configclass +class UniformNoiseCfg(NoiseCfg): + """Configuration for a additive uniform noise term.""" + + func = noise_model.uniform_noise + + n_min: torch.Tensor | float = -1.0 + """The minimum value of the noise. Defaults to -1.0.""" + n_max: torch.Tensor | float = 1.0 + """The maximum value of the noise. Defaults to 1.0.""" + + +@configclass +class GaussianNoiseCfg(NoiseCfg): + """Configuration for an additive gaussian noise term.""" + + func = noise_model.gaussian_noise + + mean: torch.Tensor | float = 0.0 + """The mean of the noise. Defaults to 0.0.""" + std: torch.Tensor | float = 1.0 + """The standard deviation of the noise. Defaults to 1.0.""" + + +## +# Noise models +## + + +@configclass +class NoiseModelCfg: + """Configuration for a noise model.""" + + class_type: type = noise_model.NoiseModel + """The class type of the noise model.""" + + noise_cfg: NoiseCfg = MISSING + """The noise configuration to use.""" + + +@configclass +class NoiseModelWithAdditiveBiasCfg(NoiseModelCfg): + """Configuration for an additive gaussian noise with bias model.""" + + class_type: type = noise_model.NoiseModelWithAdditiveBias + + bias_noise_cfg: NoiseCfg = MISSING + """The noise configuration for the bias. + + Based on this configuration, the bias is sampled at every reset of the noise model. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_model.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_model.py new file mode 100644 index 00000000000..7fceedf2e39 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/noise/noise_model.py @@ -0,0 +1,187 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from . import noise_cfg + +## +# Noise as functions. +## + + +def constant_noise(data: torch.Tensor, cfg: noise_cfg.ConstantNoiseCfg) -> torch.Tensor: + """Applies a constant noise bias to a given data set. + + Args: + data: The unmodified data set to apply noise to. + cfg: The configuration parameters for constant noise. + + Returns: + The data modified by the noise parameters provided. + """ + + # fix tensor device for bias on first call and update config parameters + if isinstance(cfg.bias, torch.Tensor): + cfg.bias = cfg.bias.to(device=data.device) + + if cfg.operation == "add": + return data + cfg.bias + elif cfg.operation == "scale": + return data * cfg.bias + elif cfg.operation == "abs": + return torch.zeros_like(data) + cfg.bias + else: + raise ValueError(f"Unknown operation in noise: {cfg.operation}") + + +def uniform_noise(data: torch.Tensor, cfg: noise_cfg.UniformNoiseCfg) -> torch.Tensor: + """Applies a uniform noise to a given data set. + + Args: + data: The unmodified data set to apply noise to. + cfg: The configuration parameters for uniform noise. + + Returns: + The data modified by the noise parameters provided. + """ + + # fix tensor device for n_max on first call and update config parameters + if isinstance(cfg.n_max, torch.Tensor): + cfg.n_max = cfg.n_max.to(data.device) + # fix tensor device for n_min on first call and update config parameters + if isinstance(cfg.n_min, torch.Tensor): + cfg.n_min = cfg.n_min.to(data.device) + + if cfg.operation == "add": + return data + torch.rand_like(data) * (cfg.n_max - cfg.n_min) + cfg.n_min + elif cfg.operation == "scale": + return data * (torch.rand_like(data) * (cfg.n_max - cfg.n_min) + cfg.n_min) + elif cfg.operation == "abs": + return torch.rand_like(data) * (cfg.n_max - cfg.n_min) + cfg.n_min + else: + raise ValueError(f"Unknown operation in noise: {cfg.operation}") + + +def gaussian_noise(data: torch.Tensor, cfg: noise_cfg.GaussianNoiseCfg) -> torch.Tensor: + """Applies a gaussian noise to a given data set. + + Args: + data: The unmodified data set to apply noise to. + cfg: The configuration parameters for gaussian noise. + + Returns: + The data modified by the noise parameters provided. + """ + + # fix tensor device for mean on first call and update config parameters + if isinstance(cfg.mean, torch.Tensor): + cfg.mean = cfg.mean.to(data.device) + # fix tensor device for std on first call and update config parameters + if isinstance(cfg.std, torch.Tensor): + cfg.std = cfg.std.to(data.device) + + if cfg.operation == "add": + return data + cfg.mean + cfg.std * torch.randn_like(data) + elif cfg.operation == "scale": + return data * (cfg.mean + cfg.std * torch.randn_like(data)) + elif cfg.operation == "abs": + return cfg.mean + cfg.std * torch.randn_like(data) + else: + raise ValueError(f"Unknown operation in noise: {cfg.operation}") + + +## +# Noise models as classes +## + + +class NoiseModel: + """Base class for noise models.""" + + def __init__(self, noise_model_cfg: noise_cfg.NoiseModelCfg, num_envs: int, device: str): + """Initialize the noise model. + + Args: + noise_model_cfg: The noise configuration to use. + num_envs: The number of environments. + device: The device to use for the noise model. + """ + self._noise_model_cfg = noise_model_cfg + self._num_envs = num_envs + self._device = device + + def reset(self, env_ids: Sequence[int] | None = None): + """Reset the noise model. + + This method can be implemented by derived classes to reset the noise model. + This is useful when implementing temporal noise models such as random walk. + + Args: + env_ids: The environment ids to reset the noise model for. Defaults to None, + in which case all environments are considered. + """ + pass + + def apply(self, data: torch.Tensor) -> torch.Tensor: + """Apply the noise to the data. + + Args: + data: The data to apply the noise to. Shape is (num_envs, ...). + + Returns: + The data with the noise applied. Shape is the same as the input data. + """ + return self._noise_model_cfg.noise_cfg.func(data, self._noise_model_cfg.noise_cfg) + + +class NoiseModelWithAdditiveBias(NoiseModel): + """Noise model with an additive bias. + + The bias term is sampled from a the specified distribution on reset. + """ + + def __init__(self, noise_model_cfg: noise_cfg.NoiseModelWithAdditiveBiasCfg, num_envs: int, device: str): + # initialize parent class + super().__init__(noise_model_cfg, num_envs, device) + # store the bias noise configuration + self._bias_noise_cfg = noise_model_cfg.bias_noise_cfg + self._bias = torch.zeros((num_envs, 1), device=self._device) + + def reset(self, env_ids: Sequence[int] | None = None): + """Reset the noise model. + + This method resets the bias term for the specified environments. + + Args: + env_ids: The environment ids to reset the noise model for. Defaults to None, + in which case all environments are considered. + """ + # resolve the environment ids + if env_ids is None: + env_ids = slice(None) + # reset the bias term + self._bias[env_ids] = self._bias_noise_cfg.func(self._bias[env_ids], self._bias_noise_cfg) + + def apply(self, data: torch.Tensor) -> torch.Tensor: + """Apply bias noise to the data. + + Args: + data: The data to apply the noise to. Shape is (num_envs, ...). + + Returns: + The data with the noise applied. Shape is the same as the input data. + """ + return super().apply(data) + self._bias diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/pretrained_checkpoint.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/pretrained_checkpoint.py new file mode 100644 index 00000000000..c5f98ad3739 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/pretrained_checkpoint.py @@ -0,0 +1,178 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for handling various pre-trained checkpoint tasks""" + +import glob +import json +import os + +import carb.settings + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry # noqa: F401 + +from .assets import retrieve_file_path + +PRETRAINED_CHECKPOINTS_ASSET_ROOT_DIR = carb.settings.get_settings().get( + "/persistent/isaaclab/asset_root/pretrained_checkpoints" +) +"""Path to the root directory on the Nucleus Server.""" + +WORKFLOWS = ["rl_games", "rsl_rl", "sb3", "skrl"] +"""The supported workflows for pre-trained checkpoints""" + +WORKFLOW_TRAINER = {w: f"scripts/reinforcement_learning/{w}/train.py" for w in WORKFLOWS} +"""A dict mapping workflow to their training program path""" + +WORKFLOW_PLAYER = {w: f"scripts/reinforcement_learning/{w}/play.py" for w in WORKFLOWS} +"""A dict mapping workflow to their play program path""" + +PRETRAINED_CHECKPOINT_PATH = str(PRETRAINED_CHECKPOINTS_ASSET_ROOT_DIR) + "/Isaac/IsaacLab/PretrainedCheckpoints" +"""URL for where we store all the pre-trained checkpoints""" + +"""The filename for checkpoints used by the different workflows""" +WORKFLOW_PRETRAINED_CHECKPOINT_FILENAMES = { + "rl_games": "checkpoint.pth", + "rsl_rl": "checkpoint.pt", + "sb3": "checkpoint.zip", + "skrl": "checkpoint.pt", +} + +"""Maps workflow to the agent variable name that determines the logging directory logs/{workflow}/{variable}""" +WORKFLOW_EXPERIMENT_NAME_VARIABLE = { + "rl_games": "agent.params.config.name", + "rsl_rl": "agent.experiment_name", + "sb3": None, + "skrl": "agent.agent.experiment.directory", +} + + +def has_pretrained_checkpoints_asset_root_dir() -> bool: + """Returns True if and only if /persistent/isaaclab/asset_root/pretrained_checkpoints exists""" + return PRETRAINED_CHECKPOINTS_ASSET_ROOT_DIR is not None + + +def get_log_root_path(workflow: str, task_name: str) -> str: + """Returns the absolute path where the logs are written for a specific workflow and task_name""" + return os.path.abspath(os.path.join("logs", workflow, task_name)) + + +def get_latest_job_run_path(workflow: str, task_name: str) -> str: + """The local logs path of the most recent run of this workflow and task name""" + log_root_path = get_log_root_path(workflow, task_name) + return _get_latest_file_or_directory(log_root_path) + + +def get_pretrained_checkpoint_path(workflow: str, task_name: str) -> str: + """The local logs path where we get the pre-trained checkpoints from""" + + path = get_latest_job_run_path(workflow, task_name) + if not path: + return None + + if workflow == "rl_games": + return os.path.join(path, "nn", f"{task_name}.pth") + elif workflow == "rsl_rl": + return _get_latest_file_or_directory(path, "*.pt") + elif workflow == "sb3": + return os.path.join(path, "model.zip") + elif workflow == "skrl": + return os.path.join(path, "checkpoints", "best_agent.pt") + else: + raise Exception(f"Unsupported workflow ({workflow})") + + +def get_pretrained_checkpoint_publish_path(workflow: str, task_name: str) -> str: + """The path where pre-trained checkpoints are published to""" + return os.path.join( + PRETRAINED_CHECKPOINT_PATH, workflow, task_name, WORKFLOW_PRETRAINED_CHECKPOINT_FILENAMES[workflow] + ) + + +def get_published_pretrained_checkpoint_path(workflow: str, task_name: str) -> str: + """The path where pre-trained checkpoints are fetched from""" + return os.path.join( + ISAACLAB_NUCLEUS_DIR, + "PretrainedCheckpoints", + workflow, + task_name, + WORKFLOW_PRETRAINED_CHECKPOINT_FILENAMES[workflow], + ) + + +def get_published_pretrained_checkpoint(workflow: str, task_name: str) -> str | None: + """Gets the path for the pre-trained checkpoint. + + If the checkpoint is not cached locally then the file is downloaded. + The cached path is then returned. + + Args: + workflow: The workflow. + task_name: The task name. + + Returns: + The path. + """ + ov_path = get_published_pretrained_checkpoint_path(workflow, task_name) + download_dir = os.path.join(".pretrained_checkpoints", workflow, task_name) + resume_path = os.path.join(download_dir, WORKFLOW_PRETRAINED_CHECKPOINT_FILENAMES[workflow]) + + if not os.path.exists(resume_path): + print(f"Fetching pre-trained checkpoint : {ov_path}") + try: + resume_path = retrieve_file_path(ov_path, download_dir) + except Exception: + print("A pre-trained checkpoint is currently unavailable for this task.") + return None + else: + print("Using pre-fetched pre-trained checkpoint") + return resume_path + + +def has_pretrained_checkpoint_job_run(workflow: str, task_name: str) -> bool: + """Returns true if an experiment exists in the logs for the workflow and task""" + return os.path.exists(get_log_root_path(workflow, task_name)) + + +def has_pretrained_checkpoint_job_finished(workflow: str, task_name: str) -> bool: + """Returns true if an experiment has results which may or may not be final depending on workflow""" + local_path = get_pretrained_checkpoint_path(workflow, task_name) + return local_path is not None and os.path.exists(local_path) + + +def get_pretrained_checkpoint_review_path(workflow: str, task_name: str) -> str | None: + """The path of the review JSON file for a workflow and task""" + run_path = get_latest_job_run_path(workflow, task_name) + if not run_path: + return None + return os.path.join(run_path, "pretrained_checkpoint_review.json") + + +def get_pretrained_checkpoint_review(workflow: str, task_name: str) -> dict | None: + """Returns the review JSON file as a dict if it exists""" + review_path = get_pretrained_checkpoint_review_path(workflow, task_name) + if not review_path: + return None + + if os.path.exists(review_path): + with open(review_path) as f: + return json.load(f) + + return None + + +def _get_latest_file_or_directory(path: str, pattern: str = "*"): + """Returns the path to the most recently modified file or directory at a path matching an optional pattern""" + g = glob.glob(f"{path}/{pattern}") + if len(g): + return max(g, key=os.path.getmtime) + return None diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/sensors.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/sensors.py new file mode 100644 index 00000000000..5c0dfcb7098 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/sensors.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import omni + + +def convert_camera_intrinsics_to_usd( + intrinsic_matrix: list[float], width: int, height: int, focal_length: float | None = None +) -> dict: + """Creates USD camera properties from camera intrinsics and resolution. + + Args: + intrinsic_matrix: Intrinsic matrix of the camera in row-major format. + The matrix is defined as [f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1]. Shape is (9,). + width: Width of the image (in pixels). + height: Height of the image (in pixels). + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None + focal_length will be calculated 1 / width. + + Returns: + A dictionary of USD camera parameters for focal_length, horizontal_aperture, vertical_aperture, + horizontal_aperture_offset, and vertical_aperture_offset. + """ + usd_params = dict + + # extract parameters from matrix + f_x = intrinsic_matrix[0] + f_y = intrinsic_matrix[4] + c_x = intrinsic_matrix[2] + c_y = intrinsic_matrix[5] + + # warn about non-square pixels + if abs(f_x - f_y) > 1e-4: + omni.log.warn("Camera non square pixels are not supported by Omniverse. The average of f_x and f_y are used.") + + # warn about aperture offsets + if abs((c_x - float(width) / 2) > 1e-4 or (c_y - float(height) / 2) > 1e-4): + omni.log.warn( + "Camera aperture offsets are not supported by Omniverse. c_x and c_y will be half of width and height" + ) + + # calculate usd camera parameters + # pixel_size does not need to be exact it is just used for consistent sizing of aperture and focal_length + # https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_camera.html#calibrated-camera-sensors + if focal_length is None: + pixel_size = 1 / float(width) + else: + pixel_size = focal_length / ((f_x + f_y) / 2) + + usd_params = { + "horizontal_aperture": pixel_size * float(width), + "vertical_aperture": pixel_size * float(height), + "focal_length": pixel_size * (f_x + f_y) / 2, # The focal length in mm + "horizontal_aperture_offset": 0.0, + "vertical_aperture_offset": 0.0, + } + + return usd_params diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/string.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/string.py new file mode 100644 index 00000000000..dda3aeaca37 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/string.py @@ -0,0 +1,373 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for transforming strings and regular expressions.""" + +import ast +import importlib +import inspect +import re +from collections.abc import Callable, Sequence +from typing import Any + +""" +String formatting. +""" + + +def to_camel_case(snake_str: str, to: str = "cC") -> str: + """Converts a string from snake case to camel case. + + Args: + snake_str: A string in snake case (i.e. with '_') + to: Convention to convert string to. Defaults to "cC". + + Raises: + ValueError: Invalid input argument `to`, i.e. not "cC" or "CC". + + Returns: + A string in camel-case format. + """ + # check input is correct + if to not in ["cC", "CC"]: + msg = "to_camel_case(): Choose a valid `to` argument (CC or cC)" + raise ValueError(msg) + # convert string to lower case and split + components = snake_str.lower().split("_") + if to == "cC": + # We capitalize the first letter of each component except the first one + # with the 'title' method and join them together. + return components[0] + "".join(x.title() for x in components[1:]) + else: + # Capitalize first letter in all the components + return "".join(x.title() for x in components) + + +def to_snake_case(camel_str: str) -> str: + """Converts a string from camel case to snake case. + + Args: + camel_str: A string in camel case. + + Returns: + A string in snake case (i.e. with '_') + """ + camel_str = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", camel_str) + return re.sub("([a-z0-9])([A-Z])", r"\1_\2", camel_str).lower() + + +def string_to_slice(s: str): + """Convert a string representation of a slice to a slice object. + + Args: + s: The string representation of the slice. + + Returns: + The slice object. + """ + # extract the content inside the slice() + match = re.match(r"slice\((.*),(.*),(.*)\)", s) + if not match: + raise ValueError(f"Invalid slice string format: {s}") + + # extract start, stop, and step values + start_str, stop_str, step_str = match.groups() + + # convert 'None' to None and other strings to integers + start = None if start_str == "None" else int(start_str) + stop = None if stop_str == "None" else int(stop_str) + step = None if step_str == "None" else int(step_str) + + # create and return the slice object + return slice(start, stop, step) + + +""" +String <-> Callable operations. +""" + + +def is_lambda_expression(name: str) -> bool: + """Checks if the input string is a lambda expression. + + Args: + name: The input string. + + Returns: + Whether the input string is a lambda expression. + """ + try: + ast.parse(name) + return isinstance(ast.parse(name).body[0], ast.Expr) and isinstance(ast.parse(name).body[0].value, ast.Lambda) + except SyntaxError: + return False + + +def callable_to_string(value: Callable) -> str: + """Converts a callable object to a string. + + Args: + value: A callable object. + + Raises: + ValueError: When the input argument is not a callable object. + + Returns: + A string representation of the callable object. + """ + # check if callable + if not callable(value): + raise ValueError(f"The input argument is not callable: {value}.") + # check if lambda function + if value.__name__ == "": + # we resolve the lambda expression by checking the source code and extracting the line with lambda expression + # we also remove any comments from the line + lambda_line = inspect.getsourcelines(value)[0][0].strip().split("lambda")[1].strip().split(",")[0] + lambda_line = re.sub(r"#.*$", "", lambda_line).rstrip() + return f"lambda {lambda_line}" + else: + # get the module and function name + module_name = value.__module__ + function_name = value.__name__ + # return the string + return f"{module_name}:{function_name}" + + +def string_to_callable(name: str) -> Callable: + """Resolves the module and function names to return the function. + + Args: + name: The function name. The format should be 'module:attribute_name' or a + lambda expression of format: 'lambda x: x'. + + Raises: + ValueError: When the resolved attribute is not a function. + ValueError: When the module cannot be found. + + Returns: + Callable: The function loaded from the module. + """ + try: + if is_lambda_expression(name): + callable_object = eval(name) + else: + mod_name, attr_name = name.split(":") + mod = importlib.import_module(mod_name) + callable_object = getattr(mod, attr_name) + # check if attribute is callable + if callable(callable_object): + return callable_object + else: + raise AttributeError(f"The imported object is not callable: '{name}'") + except (ValueError, ModuleNotFoundError) as e: + msg = ( + f"Could not resolve the input string '{name}' into callable object." + " The format of input should be 'module:attribute_name'.\n" + f"Received the error:\n {e}." + ) + raise ValueError(msg) + + +""" +Regex operations. +""" + + +def resolve_matching_names( + keys: str | Sequence[str], list_of_strings: Sequence[str], preserve_order: bool = False +) -> tuple[list[int], list[str]]: + """Match a list of query regular expressions against a list of strings and return the matched indices and names. + + When a list of query regular expressions is provided, the function checks each target string against each + query regular expression and returns the indices of the matched strings and the matched strings. + + If the :attr:`preserve_order` is True, the ordering of the matched indices and names is the same as the order + of the provided list of strings. This means that the ordering is dictated by the order of the target strings + and not the order of the query regular expressions. + + If the :attr:`preserve_order` is False, the ordering of the matched indices and names is the same as the order + of the provided list of query regular expressions. + + For example, consider the list of strings is ['a', 'b', 'c', 'd', 'e'] and the regular expressions are ['a|c', 'b']. + If :attr:`preserve_order` is False, then the function will return the indices of the matched strings and the + strings as: ([0, 1, 2], ['a', 'b', 'c']). When :attr:`preserve_order` is True, it will return them as: + ([0, 2, 1], ['a', 'c', 'b']). + + Note: + The function does not sort the indices. It returns the indices in the order they are found. + + Args: + keys: A regular expression or a list of regular expressions to match the strings in the list. + list_of_strings: A list of strings to match. + preserve_order: Whether to preserve the order of the query keys in the returned values. Defaults to False. + + Returns: + A tuple of lists containing the matched indices and names. + + Raises: + ValueError: When multiple matches are found for a string in the list. + ValueError: When not all regular expressions are matched. + """ + # resolve name keys + if isinstance(keys, str): + keys = [keys] + # find matching patterns + index_list = [] + names_list = [] + key_idx_list = [] + # book-keeping to check that we always have a one-to-one mapping + # i.e. each target string should match only one regular expression + target_strings_match_found = [None for _ in range(len(list_of_strings))] + keys_match_found = [[] for _ in range(len(keys))] + # loop over all target strings + for target_index, potential_match_string in enumerate(list_of_strings): + for key_index, re_key in enumerate(keys): + if re.fullmatch(re_key, potential_match_string): + # check if match already found + if target_strings_match_found[target_index]: + raise ValueError( + f"Multiple matches for '{potential_match_string}':" + f" '{target_strings_match_found[target_index]}' and '{re_key}'!" + ) + # add to list + target_strings_match_found[target_index] = re_key + index_list.append(target_index) + names_list.append(potential_match_string) + key_idx_list.append(key_index) + # add for regex key + keys_match_found[key_index].append(potential_match_string) + # reorder keys if they should be returned in order of the query keys + if preserve_order: + reordered_index_list = [None] * len(index_list) + global_index = 0 + for key_index in range(len(keys)): + for key_idx_position, key_idx_entry in enumerate(key_idx_list): + if key_idx_entry == key_index: + reordered_index_list[key_idx_position] = global_index + global_index += 1 + # reorder index and names list + index_list_reorder = [None] * len(index_list) + names_list_reorder = [None] * len(index_list) + for idx, reorder_idx in enumerate(reordered_index_list): + index_list_reorder[reorder_idx] = index_list[idx] + names_list_reorder[reorder_idx] = names_list[idx] + # update + index_list = index_list_reorder + names_list = names_list_reorder + # check that all regular expressions are matched + if not all(keys_match_found): + # make this print nicely aligned for debugging + msg = "\n" + for key, value in zip(keys, keys_match_found): + msg += f"\t{key}: {value}\n" + msg += f"Available strings: {list_of_strings}\n" + # raise error + raise ValueError( + f"Not all regular expressions are matched! Please check that the regular expressions are correct: {msg}" + ) + # return + return index_list, names_list + + +def resolve_matching_names_values( + data: dict[str, Any], list_of_strings: Sequence[str], preserve_order: bool = False +) -> tuple[list[int], list[str], list[Any]]: + """Match a list of regular expressions in a dictionary against a list of strings and return + the matched indices, names, and values. + + If the :attr:`preserve_order` is True, the ordering of the matched indices and names is the same as the order + of the provided list of strings. This means that the ordering is dictated by the order of the target strings + and not the order of the query regular expressions. + + If the :attr:`preserve_order` is False, the ordering of the matched indices and names is the same as the order + of the provided list of query regular expressions. + + For example, consider the dictionary is {"a|d|e": 1, "b|c": 2}, the list of strings is ['a', 'b', 'c', 'd', 'e']. + If :attr:`preserve_order` is False, then the function will return the indices of the matched strings, the + matched strings, and the values as: ([0, 1, 2, 3, 4], ['a', 'b', 'c', 'd', 'e'], [1, 2, 2, 1, 1]). When + :attr:`preserve_order` is True, it will return them as: ([0, 3, 4, 1, 2], ['a', 'd', 'e', 'b', 'c'], [1, 1, 1, 2, 2]). + + Args: + data: A dictionary of regular expressions and values to match the strings in the list. + list_of_strings: A list of strings to match. + preserve_order: Whether to preserve the order of the query keys in the returned values. Defaults to False. + + Returns: + A tuple of lists containing the matched indices, names, and values. + + Raises: + TypeError: When the input argument :attr:`data` is not a dictionary. + ValueError: When multiple matches are found for a string in the dictionary. + ValueError: When not all regular expressions in the data keys are matched. + """ + # check valid input + if not isinstance(data, dict): + raise TypeError(f"Input argument `data` should be a dictionary. Received: {data}") + # find matching patterns + index_list = [] + names_list = [] + values_list = [] + key_idx_list = [] + # book-keeping to check that we always have a one-to-one mapping + # i.e. each target string should match only one regular expression + target_strings_match_found = [None for _ in range(len(list_of_strings))] + keys_match_found = [[] for _ in range(len(data))] + # loop over all target strings + for target_index, potential_match_string in enumerate(list_of_strings): + for key_index, (re_key, value) in enumerate(data.items()): + if re.fullmatch(re_key, potential_match_string): + # check if match already found + if target_strings_match_found[target_index]: + raise ValueError( + f"Multiple matches for '{potential_match_string}':" + f" '{target_strings_match_found[target_index]}' and '{re_key}'!" + ) + # add to list + target_strings_match_found[target_index] = re_key + index_list.append(target_index) + names_list.append(potential_match_string) + values_list.append(value) + key_idx_list.append(key_index) + # add for regex key + keys_match_found[key_index].append(potential_match_string) + # reorder keys if they should be returned in order of the query keys + if preserve_order: + reordered_index_list = [None] * len(index_list) + global_index = 0 + for key_index in range(len(data)): + for key_idx_position, key_idx_entry in enumerate(key_idx_list): + if key_idx_entry == key_index: + reordered_index_list[key_idx_position] = global_index + global_index += 1 + # reorder index and names list + index_list_reorder = [None] * len(index_list) + names_list_reorder = [None] * len(index_list) + values_list_reorder = [None] * len(index_list) + for idx, reorder_idx in enumerate(reordered_index_list): + index_list_reorder[reorder_idx] = index_list[idx] + names_list_reorder[reorder_idx] = names_list[idx] + values_list_reorder[reorder_idx] = values_list[idx] + # update + index_list = index_list_reorder + names_list = names_list_reorder + values_list = values_list_reorder + # check that all regular expressions are matched + if not all(keys_match_found): + # make this print nicely aligned for debugging + msg = "\n" + for key, value in zip(data.keys(), keys_match_found): + msg += f"\t{key}: {value}\n" + msg += f"Available strings: {list_of_strings}\n" + # raise error + raise ValueError( + f"Not all regular expressions are matched! Please check that the regular expressions are correct: {msg}" + ) + # return + return index_list, names_list, values_list diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/timer.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/timer.py new file mode 100644 index 00000000000..d4ea07f5b49 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/timer.py @@ -0,0 +1,176 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for a timer class that can be used for performance measurements.""" + +from __future__ import annotations + +import time +from contextlib import ContextDecorator +from typing import Any, ClassVar + + +class TimerError(Exception): + """A custom exception used to report errors in use of :class:`Timer` class.""" + + pass + + +class Timer(ContextDecorator): + """A timer for performance measurements. + + A class to keep track of time for performance measurement. + It allows timing via context managers and decorators as well. + + It uses the `time.perf_counter` function to measure time. This function + returns the number of seconds since the epoch as a float. It has the + highest resolution available on the system. + + As a regular object: + + .. code-block:: python + + import time + + from isaaclab.utils.timer import Timer + + timer = Timer() + timer.start() + time.sleep(1) + print(1 <= timer.time_elapsed <= 2) # Output: True + + time.sleep(1) + timer.stop() + print(2 <= stopwatch.total_run_time) # Output: True + + As a context manager: + + .. code-block:: python + + import time + + from isaaclab.utils.timer import Timer + + with Timer() as timer: + time.sleep(1) + print(1 <= timer.time_elapsed <= 2) # Output: True + + Reference: https://gist.github.com/sumeet/1123871 + """ + + timing_info: ClassVar[dict[str, float]] = dict() + """Dictionary for storing the elapsed time per timer instances globally. + + This dictionary logs the timer information. The keys are the names given to the timer class + at its initialization. If no :attr:`name` is passed to the constructor, no time + is recorded in the dictionary. + """ + + def __init__(self, msg: str | None = None, name: str | None = None): + """Initializes the timer. + + Args: + msg: The message to display when using the timer + class in a context manager. Defaults to None. + name: The name to use for logging times in a global + dictionary. Defaults to None. + """ + self._msg = msg + self._name = name + self._start_time = None + self._stop_time = None + self._elapsed_time = None + + def __str__(self) -> str: + """A string representation of the class object. + + Returns: + A string containing the elapsed time. + """ + return f"{self.time_elapsed:0.6f} seconds" + + """ + Properties + """ + + @property + def time_elapsed(self) -> float: + """The number of seconds that have elapsed since this timer started timing. + + Note: + This is used for checking how much time has elapsed while the timer is still running. + """ + return time.perf_counter() - self._start_time + + @property + def total_run_time(self) -> float: + """The number of seconds that elapsed from when the timer started to when it ended.""" + return self._elapsed_time + + """ + Operations + """ + + def start(self): + """Start timing.""" + if self._start_time is not None: + raise TimerError("Timer is running. Use .stop() to stop it") + + self._start_time = time.perf_counter() + + def stop(self): + """Stop timing.""" + if self._start_time is None: + raise TimerError("Timer is not running. Use .start() to start it") + + self._stop_time = time.perf_counter() + self._elapsed_time = self._stop_time - self._start_time + self._start_time = None + + if self._name: + Timer.timing_info[self._name] = self._elapsed_time + + """ + Context managers + """ + + def __enter__(self) -> Timer: + """Start timing and return this `Timer` instance.""" + self.start() + return self + + def __exit__(self, *exc_info: Any): + """Stop timing.""" + self.stop() + # print message + if self._msg is not None: + print(self._msg, f": {self._elapsed_time:0.6f} seconds") + + """ + Static Methods + """ + + @staticmethod + def get_timer_info(name: str) -> float: + """Retrieves the time logged in the global dictionary + based on name. + + Args: + name: Name of the the entry to be retrieved. + + Raises: + TimerError: If name doesn't exist in the log. + + Returns: + A float containing the time logged if the name exists. + """ + if name not in Timer.timing_info: + raise TimerError(f"Timer {name} does not exist") + return Timer.timing_info.get(name) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/types.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/types.py new file mode 100644 index 00000000000..7b20e8281ed --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/types.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for different data types.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from dataclasses import dataclass + + +@dataclass +class ArticulationActions: + """Data container to store articulation's joints actions. + + This class is used to store the actions of the joints of an articulation. + It is used to store the joint positions, velocities, efforts, and indices. + + If the actions are not provided, the values are set to None. + """ + + joint_positions: torch.Tensor | None = None + """The joint positions of the articulation. Defaults to None.""" + + joint_velocities: torch.Tensor | None = None + """The joint velocities of the articulation. Defaults to None.""" + + joint_efforts: torch.Tensor | None = None + """The joint efforts of the articulation. Defaults to None.""" + + joint_indices: torch.Tensor | Sequence[int] | slice | None = None + """The joint indices of the articulation. Defaults to None. + + If the joint indices are a slice, this indicates that the indices are continuous and correspond + to all the joints of the articulation. We use a slice to make the indexing more efficient. + """ diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/__init__.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/__init__.py new file mode 100644 index 00000000000..53dbc162434 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing operations based on warp.""" + +from .ops import convert_to_warp_mesh, raycast_mesh diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/kernels.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/kernels.py new file mode 100644 index 00000000000..d69149e1c33 --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/kernels.py @@ -0,0 +1,138 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Custom kernels for warp.""" + +from typing import Any + +import warp as wp + + +@wp.kernel(enable_backward=False) +def raycast_mesh_kernel( + mesh: wp.uint64, + ray_starts: wp.array(dtype=wp.vec3), + ray_directions: wp.array(dtype=wp.vec3), + ray_hits: wp.array(dtype=wp.vec3), + ray_distance: wp.array(dtype=wp.float32), + ray_normal: wp.array(dtype=wp.vec3), + ray_face_id: wp.array(dtype=wp.int32), + max_dist: float = 1e6, + return_distance: int = False, + return_normal: int = False, + return_face_id: int = False, +): + """Performs ray-casting against a mesh. + + This function performs ray-casting against the given mesh using the provided ray start positions + and directions. The resulting ray hit positions are stored in the :obj:`ray_hits` array. + + Note that the `ray_starts`, `ray_directions`, and `ray_hits` arrays should have compatible shapes + and data types to ensure proper execution. Additionally, they all must be in the same frame. + + The function utilizes the `mesh_query_ray` method from the `wp` module to perform the actual ray-casting + operation. The maximum ray-cast distance is set to `1e6` units. + + Args: + mesh: The input mesh. The ray-casting is performed against this mesh on the device specified by the + `mesh`'s `device` attribute. + ray_starts: The input ray start positions. Shape is (N, 3). + ray_directions: The input ray directions. Shape is (N, 3). + ray_hits: The output ray hit positions. Shape is (N, 3). + ray_distance: The output ray hit distances. Shape is (N,), if `return_distance` is True. Otherwise, + this array is not used. + ray_normal: The output ray hit normals. Shape is (N, 3), if `return_normal` is True. Otherwise, + this array is not used. + ray_face_id: The output ray hit face ids. Shape is (N,), if `return_face_id` is True. Otherwise, + this array is not used. + max_dist: The maximum ray-cast distance. Defaults to 1e6. + return_distance: Whether to return the ray hit distances. Defaults to False. + return_normal: Whether to return the ray hit normals. Defaults to False`. + return_face_id: Whether to return the ray hit face ids. Defaults to False. + """ + # get the thread id + tid = wp.tid() + + t = float(0.0) # hit distance along ray + u = float(0.0) # hit face barycentric u + v = float(0.0) # hit face barycentric v + sign = float(0.0) # hit face sign + n = wp.vec3() # hit face normal + f = int(0) # hit face index + + # ray cast against the mesh and store the hit position + hit_success = wp.mesh_query_ray(mesh, ray_starts[tid], ray_directions[tid], max_dist, t, u, v, sign, n, f) + # if the ray hit, store the hit data + if hit_success: + ray_hits[tid] = ray_starts[tid] + t * ray_directions[tid] + if return_distance == 1: + ray_distance[tid] = t + if return_normal == 1: + ray_normal[tid] = n + if return_face_id == 1: + ray_face_id[tid] = f + + +@wp.kernel(enable_backward=False) +def reshape_tiled_image( + tiled_image_buffer: Any, + batched_image: Any, + image_height: int, + image_width: int, + num_channels: int, + num_tiles_x: int, +): + """Reshapes a tiled image into a batch of images. + + This function reshapes the input tiled image buffer into a batch of images. The input image buffer + is assumed to be tiled in the x and y directions. The output image is a batch of images with the + specified height, width, and number of channels. + + Args: + tiled_image_buffer: The input image buffer. Shape is (height * width * num_channels * num_cameras,). + batched_image: The output image. Shape is (num_cameras, height, width, num_channels). + image_width: The width of the image. + image_height: The height of the image. + num_channels: The number of channels in the image. + num_tiles_x: The number of tiles in x-direction. + """ + # get the thread id + camera_id, height_id, width_id = wp.tid() + + # resolve the tile indices + tile_x_id = camera_id % num_tiles_x + tile_y_id = camera_id // num_tiles_x + # compute the start index of the pixel in the tiled image buffer + pixel_start = ( + num_channels * num_tiles_x * image_width * (image_height * tile_y_id + height_id) + + num_channels * tile_x_id * image_width + + num_channels * width_id + ) + + # copy the pixel values into the batched image + for i in range(num_channels): + batched_image[camera_id, height_id, width_id, i] = batched_image.dtype(tiled_image_buffer[pixel_start + i]) + + +# uint32 -> int32 conversion is required for non-colored segmentation annotators +wp.overload( + reshape_tiled_image, + {"tiled_image_buffer": wp.array(dtype=wp.uint32), "batched_image": wp.array(dtype=wp.uint32, ndim=4)}, +) +# uint8 is used for 4 channel annotators +wp.overload( + reshape_tiled_image, + {"tiled_image_buffer": wp.array(dtype=wp.uint8), "batched_image": wp.array(dtype=wp.uint8, ndim=4)}, +) +# float32 is used for single channel annotators +wp.overload( + reshape_tiled_image, + {"tiled_image_buffer": wp.array(dtype=wp.float32), "batched_image": wp.array(dtype=wp.float32, ndim=4)}, +) diff --git a/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/ops.py b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/ops.py new file mode 100644 index 00000000000..b5442f8828a --- /dev/null +++ b/install/isaaclab/lib/python3.10/site-packages/isaaclab/utils/warp/ops.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapping around warp kernels for compatibility with torch tensors.""" + +# needed to import for allowing type-hinting: torch.Tensor | None +from __future__ import annotations + +import numpy as np +import torch + +import warp as wp + +# disable warp module initialization messages +wp.config.quiet = True +# initialize the warp module +wp.init() + +from . import kernels + + +def raycast_mesh( + ray_starts: torch.Tensor, + ray_directions: torch.Tensor, + mesh: wp.Mesh, + max_dist: float = 1e6, + return_distance: bool = False, + return_normal: bool = False, + return_face_id: bool = False, +) -> tuple[torch.Tensor, torch.Tensor | None, torch.Tensor | None, torch.Tensor | None]: + """Performs ray-casting against a mesh. + + Note that the `ray_starts` and `ray_directions`, and `ray_hits` should have compatible shapes + and data types to ensure proper execution. Additionally, they all must be in the same frame. + + Args: + ray_starts: The starting position of the rays. Shape (N, 3). + ray_directions: The ray directions for each ray. Shape (N, 3). + mesh: The warp mesh to ray-cast against. + max_dist: The maximum distance to ray-cast. Defaults to 1e6. + return_distance: Whether to return the distance of the ray until it hits the mesh. Defaults to False. + return_normal: Whether to return the normal of the mesh face the ray hits. Defaults to False. + return_face_id: Whether to return the face id of the mesh face the ray hits. Defaults to False. + + Returns: + The ray hit position. Shape (N, 3). + The returned tensor contains :obj:`float('inf')` for missed hits. + The ray hit distance. Shape (N,). + Will only return if :attr:`return_distance` is True, else returns None. + The returned tensor contains :obj:`float('inf')` for missed hits. + The ray hit normal. Shape (N, 3). + Will only return if :attr:`return_normal` is True else returns None. + The returned tensor contains :obj:`float('inf')` for missed hits. + The ray hit face id. Shape (N,). + Will only return if :attr:`return_face_id` is True else returns None. + The returned tensor contains :obj:`int(-1)` for missed hits. + """ + # extract device and shape information + shape = ray_starts.shape + device = ray_starts.device + # device of the mesh + torch_device = wp.device_to_torch(mesh.device) + # reshape the tensors + ray_starts = ray_starts.to(torch_device).view(-1, 3).contiguous() + ray_directions = ray_directions.to(torch_device).view(-1, 3).contiguous() + num_rays = ray_starts.shape[0] + # create output tensor for the ray hits + ray_hits = torch.full((num_rays, 3), float("inf"), device=torch_device).contiguous() + + # map the memory to warp arrays + ray_starts_wp = wp.from_torch(ray_starts, dtype=wp.vec3) + ray_directions_wp = wp.from_torch(ray_directions, dtype=wp.vec3) + ray_hits_wp = wp.from_torch(ray_hits, dtype=wp.vec3) + + if return_distance: + ray_distance = torch.full((num_rays,), float("inf"), device=torch_device).contiguous() + ray_distance_wp = wp.from_torch(ray_distance, dtype=wp.float32) + else: + ray_distance = None + ray_distance_wp = wp.empty((1,), dtype=wp.float32, device=torch_device) + + if return_normal: + ray_normal = torch.full((num_rays, 3), float("inf"), device=torch_device).contiguous() + ray_normal_wp = wp.from_torch(ray_normal, dtype=wp.vec3) + else: + ray_normal = None + ray_normal_wp = wp.empty((1,), dtype=wp.vec3, device=torch_device) + + if return_face_id: + ray_face_id = torch.ones((num_rays,), dtype=torch.int32, device=torch_device).contiguous() * (-1) + ray_face_id_wp = wp.from_torch(ray_face_id, dtype=wp.int32) + else: + ray_face_id = None + ray_face_id_wp = wp.empty((1,), dtype=wp.int32, device=torch_device) + + # launch the warp kernel + wp.launch( + kernel=kernels.raycast_mesh_kernel, + dim=num_rays, + inputs=[ + mesh.id, + ray_starts_wp, + ray_directions_wp, + ray_hits_wp, + ray_distance_wp, + ray_normal_wp, + ray_face_id_wp, + float(max_dist), + int(return_distance), + int(return_normal), + int(return_face_id), + ], + device=mesh.device, + ) + # NOTE: Synchronize is not needed anymore, but we keep it for now. Check with @dhoeller. + wp.synchronize() + + if return_distance: + ray_distance = ray_distance.to(device).view(shape[0], shape[1]) + if return_normal: + ray_normal = ray_normal.to(device).view(shape) + if return_face_id: + ray_face_id = ray_face_id.to(device).view(shape[0], shape[1]) + + return ray_hits.to(device).view(shape), ray_distance, ray_normal, ray_face_id + + +def convert_to_warp_mesh(points: np.ndarray, indices: np.ndarray, device: str) -> wp.Mesh: + """Create a warp mesh object with a mesh defined from vertices and triangles. + + Args: + points: The vertices of the mesh. Shape is (N, 3), where N is the number of vertices. + indices: The triangles of the mesh as references to vertices for each triangle. + Shape is (M, 3), where M is the number of triangles / faces. + device: The device to use for the mesh. + + Returns: + The warp mesh object. + """ + return wp.Mesh( + points=wp.array(points.astype(np.float32), dtype=wp.vec3, device=device), + indices=wp.array(indices.astype(np.int32).flatten(), dtype=wp.int32, device=device), + ) diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/__init__.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/__init__.py new file mode 100644 index 00000000000..6500aa5fafb --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/__init__.py @@ -0,0 +1,29 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""Package containing asset and sensor configurations.""" + +import os +import toml + +# Conveniences to other module directories via relative paths +ISAACLAB_ASSETS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +"""Path to the extension source directory.""" + +ISAACLAB_ASSETS_DATA_DIR = os.path.join(ISAACLAB_ASSETS_EXT_DIR, "data") +"""Path to the extension data directory.""" + +ISAACLAB_ASSETS_METADATA = toml.load(os.path.join(ISAACLAB_ASSETS_EXT_DIR, "config", "extension.toml")) +"""Extension metadata dictionary parsed from the extension.toml file.""" + +# Configure the module-level variables +__version__ = ISAACLAB_ASSETS_METADATA["package"]["version"] + +from .robots import * +from .sensors import * diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/__init__.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/__init__.py new file mode 100644 index 00000000000..38574eac582 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/__init__.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +## +# Configuration for different assets. +## + +from .allegro import * +from .ant import * +from .anymal import * +from .cart_double_pendulum import * +from .cartpole import * +from .fourier import * +from .franka import * +from .humanoid import * +from .humanoid_28 import * +from .kinova import * +from .quadcopter import * +from .ridgeback_franka import * +from .sawyer import * +from .shadow_hand import * +from .spot import * +from .unitree import * +from .universal_robots import * diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/allegro.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/allegro.py new file mode 100644 index 00000000000..a3302aeae0a --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/allegro.py @@ -0,0 +1,75 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Allegro Hand robots from Wonik Robotics. + +The following configurations are available: + +* :obj:`ALLEGRO_HAND_CFG`: Allegro Hand with implicit actuator model. + +Reference: + +* https://www.wonikrobotics.com/robot-hand + +""" + + +import math + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +ALLEGRO_HAND_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/AllegroHand/allegro_hand_instanceable.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=False, + enable_gyroscopic_forces=False, + angular_damping=0.01, + max_linear_velocity=1000.0, + max_angular_velocity=64 / math.pi * 180.0, + max_depenetration_velocity=1000.0, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0005, + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(0.257551, 0.283045, 0.683330, -0.621782), + joint_pos={"^(?!thumb_joint_0).*": 0.0, "thumb_joint_0": 0.28}, + ), + actuators={ + "fingers": ImplicitActuatorCfg( + joint_names_expr=[".*"], + effort_limit=0.5, + velocity_limit=100.0, + stiffness=3.0, + damping=0.1, + friction=0.01, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of Allegro Hand robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ant.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ant.py new file mode 100644 index 00000000000..9a8362d7045 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ant.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Mujoco Ant robot.""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +ANT_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Ant/ant_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=10.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + joint_pos={ + ".*_leg": 0.0, + "front_left_foot": 0.785398, # 45 degrees + "front_right_foot": -0.785398, + "left_back_foot": -0.785398, + "right_back_foot": 0.785398, + }, + ), + actuators={ + "body": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.0, + damping=0.0, + ), + }, +) +"""Configuration for the Mujoco Ant robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/anymal.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/anymal.py new file mode 100644 index 00000000000..744dcf9d5d6 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/anymal.py @@ -0,0 +1,180 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the ANYbotics robots. + +The following configuration parameters are available: + +* :obj:`ANYMAL_B_CFG`: The ANYmal-B robot with ANYdrives 3.0 +* :obj:`ANYMAL_C_CFG`: The ANYmal-C robot with ANYdrives 3.0 +* :obj:`ANYMAL_D_CFG`: The ANYmal-D robot with ANYdrives 3.0 + +Reference: + +* https://github.com/ANYbotics/anymal_b_simple_description +* https://github.com/ANYbotics/anymal_c_simple_description +* https://github.com/ANYbotics/anymal_d_simple_description + +""" + +from isaaclab_assets.sensors.velodyne import VELODYNE_VLP_16_RAYCASTER_CFG + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ActuatorNetLSTMCfg, DCMotorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.sensors import RayCasterCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration - Actuators. +## + +ANYDRIVE_3_SIMPLE_ACTUATOR_CFG = DCMotorCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + saturation_effort=120.0, + effort_limit=80.0, + velocity_limit=7.5, + stiffness={".*": 40.0}, + damping={".*": 5.0}, +) +"""Configuration for ANYdrive 3.x with DC actuator model.""" + + +ANYDRIVE_3_LSTM_ACTUATOR_CFG = ActuatorNetLSTMCfg( + joint_names_expr=[".*HAA", ".*HFE", ".*KFE"], + network_file=f"{ISAACLAB_NUCLEUS_DIR}/ActuatorNets/ANYbotics/anydrive_3_lstm_jit.pt", + saturation_effort=120.0, + effort_limit=80.0, + velocity_limit=7.5, +) +"""Configuration for ANYdrive 3.0 (used on ANYmal-C) with LSTM actuator model.""" + + +## +# Configuration - Articulation. +## + +ANYMAL_B_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-B/anymal_b.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.6), + joint_pos={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, # both front KFE + ".*H_KFE": 0.8, # both hind KFE + }, + ), + actuators={"legs": ANYDRIVE_3_LSTM_ACTUATOR_CFG}, + soft_joint_pos_limit_factor=0.95, +) +"""Configuration of ANYmal-B robot using actuator-net.""" + + +ANYMAL_C_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd", + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.6), + joint_pos={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, # both front KFE + ".*H_KFE": 0.8, # both hind KFE + }, + ), + actuators={"legs": ANYDRIVE_3_LSTM_ACTUATOR_CFG}, + soft_joint_pos_limit_factor=0.95, +) +"""Configuration of ANYmal-C robot using actuator-net.""" + + +ANYMAL_D_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d.usd", + # usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-D/anymal_d_minimal.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.02, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.6), + joint_pos={ + ".*HAA": 0.0, # all HAA + ".*F_HFE": 0.4, # both front HFE + ".*H_HFE": -0.4, # both hind HFE + ".*F_KFE": -0.8, # both front KFE + ".*H_KFE": 0.8, # both hind KFE + }, + ), + actuators={"legs": ANYDRIVE_3_LSTM_ACTUATOR_CFG}, + soft_joint_pos_limit_factor=0.95, +) +"""Configuration of ANYmal-D robot using actuator-net. + +Note: + Since we don't have a publicly available actuator network for ANYmal-D, we use the same network as ANYmal-C. + This may impact the sim-to-real transfer performance. +""" + + +## +# Configuration - Sensors. +## + +ANYMAL_LIDAR_CFG = VELODYNE_VLP_16_RAYCASTER_CFG.replace( + offset=RayCasterCfg.OffsetCfg(pos=(-0.310, 0.000, 0.159), rot=(0.0, 0.0, 0.0, 1.0)) +) +"""Configuration for the Velodyne VLP-16 sensor mounted on the ANYmal robot's base.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cart_double_pendulum.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cart_double_pendulum.py new file mode 100644 index 00000000000..9783e1737ce --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cart_double_pendulum.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for a simple inverted Double Pendulum on a Cart robot.""" + + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +CART_DOUBLE_PENDULUM_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Classic/CartDoublePendulum/cart_double_pendulum.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=100.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 2.0), joint_pos={"slider_to_cart": 0.0, "cart_to_pole": 0.0, "pole_to_pendulum": 0.0} + ), + actuators={ + "cart_actuator": ImplicitActuatorCfg( + joint_names_expr=["slider_to_cart"], + effort_limit=400.0, + velocity_limit=100.0, + stiffness=0.0, + damping=10.0, + ), + "pole_actuator": ImplicitActuatorCfg( + joint_names_expr=["cart_to_pole"], effort_limit=400.0, velocity_limit=100.0, stiffness=0.0, damping=0.0 + ), + "pendulum_actuator": ImplicitActuatorCfg( + joint_names_expr=["pole_to_pendulum"], effort_limit=400.0, velocity_limit=100.0, stiffness=0.0, damping=0.0 + ), + }, +) +"""Configuration for a simple inverted Double Pendulum on a Cart robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cartpole.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cartpole.py new file mode 100644 index 00000000000..8c548d317c0 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cartpole.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for a simple Cartpole robot.""" + + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +CARTPOLE_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Classic/Cartpole/cartpole.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=100.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 2.0), joint_pos={"slider_to_cart": 0.0, "cart_to_pole": 0.0} + ), + actuators={ + "cart_actuator": ImplicitActuatorCfg( + joint_names_expr=["slider_to_cart"], + effort_limit=400.0, + velocity_limit=100.0, + stiffness=0.0, + damping=10.0, + ), + "pole_actuator": ImplicitActuatorCfg( + joint_names_expr=["cart_to_pole"], effort_limit=400.0, velocity_limit=100.0, stiffness=0.0, damping=0.0 + ), + }, +) +"""Configuration for a simple Cartpole robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cassie.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cassie.py new file mode 100644 index 00000000000..a02148394e8 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/cassie.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Agility robots. + +The following configurations are available: + +* :obj:`CASSIE_CFG`: Agility Cassie robot with simple PD controller for the legs + +Reference: https://github.com/UMich-BipedLab/Cassie_Model/blob/master/urdf/cassie.urdf +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +CASSIE_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Agility/Cassie/cassie.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.9), + joint_pos={ + "hip_abduction_left": 0.1, + "hip_rotation_left": 0.0, + "hip_flexion_left": 1.0, + "thigh_joint_left": -1.8, + "ankle_joint_left": 1.57, + "toe_joint_left": -1.57, + "hip_abduction_right": -0.1, + "hip_rotation_right": 0.0, + "hip_flexion_right": 1.0, + "thigh_joint_right": -1.8, + "ankle_joint_right": 1.57, + "toe_joint_right": -1.57, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=["hip_.*", "thigh_.*", "ankle_.*"], + effort_limit=200.0, + velocity_limit=10.0, + stiffness={ + "hip_abduction.*": 100.0, + "hip_rotation.*": 100.0, + "hip_flexion.*": 200.0, + "thigh_joint.*": 200.0, + "ankle_joint.*": 200.0, + }, + damping={ + "hip_abduction.*": 3.0, + "hip_rotation.*": 3.0, + "hip_flexion.*": 6.0, + "thigh_joint.*": 6.0, + "ankle_joint.*": 6.0, + }, + ), + "toes": ImplicitActuatorCfg( + joint_names_expr=["toe_.*"], + effort_limit=20.0, + velocity_limit=10.0, + stiffness={ + "toe_joint.*": 20.0, + }, + damping={ + "toe_joint.*": 1.0, + }, + ), + }, +) diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/fourier.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/fourier.py new file mode 100644 index 00000000000..de7b733cfed --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/fourier.py @@ -0,0 +1,130 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Fourier Robots. + +The following configuration parameters are available: + +* :obj:`GR1T2_CFG`: The GR1T2 humanoid. + +Reference: https://www.fftai.com/products-gr1 +""" + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + + +GR1T2_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=( + f"{ISAAC_NUCLEUS_DIR}/Robots/FourierIntelligence/GR-1/GR1T2_fourier_hand_6dof/GR1T2_fourier_hand_6dof.usd" + ), + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.95), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, + ), + actuators={ + "head": ImplicitActuatorCfg( + joint_names_expr=[ + "head_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "trunk": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_hip_.*", + ".*_knee_.*", + ".*_ankle_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "right-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "right_shoulder_.*", + "right_elbow_.*", + "right_wrist_.*", + ], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "left-arm": ImplicitActuatorCfg( + joint_names_expr=[ + "left_shoulder_.*", + "left_elbow_.*", + "left_wrist_.*", + ], + effort_limit=torch.inf, + velocity_limit=torch.inf, + stiffness=None, + damping=None, + armature=0.0, + ), + "right-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "R_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + "left-hand": ImplicitActuatorCfg( + joint_names_expr=[ + "L_.*", + ], + effort_limit=None, + velocity_limit=None, + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the GR1T2 Humanoid robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/franka.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/franka.py new file mode 100644 index 00000000000..0e778a3094b --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/franka.py @@ -0,0 +1,92 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Franka Emika robots. + +The following configurations are available: + +* :obj:`FRANKA_PANDA_CFG`: Franka Emika Panda robot with Panda hand +* :obj:`FRANKA_PANDA_HIGH_PD_CFG`: Franka Emika Panda robot with Panda hand with stiffer PD control + +Reference: https://github.com/frankaemika/franka_ros +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +FRANKA_PANDA_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + "panda_joint5": 0.0, + "panda_joint6": 3.037, + "panda_joint7": 0.741, + "panda_finger_joint.*": 0.04, + }, + ), + actuators={ + "panda_shoulder": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + effort_limit_sim=87.0, + velocity_limit_sim=2.175, + stiffness=80.0, + damping=4.0, + ), + "panda_forearm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + effort_limit_sim=12.0, + velocity_limit_sim=2.61, + stiffness=80.0, + damping=4.0, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint.*"], + effort_limit_sim=200.0, + velocity_limit_sim=0.2, + stiffness=2e3, + damping=1e2, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of Franka Emika Panda robot.""" + + +FRANKA_PANDA_HIGH_PD_CFG = FRANKA_PANDA_CFG.copy() +FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].stiffness = 400.0 +FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].damping = 80.0 +FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].stiffness = 400.0 +FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].damping = 80.0 +"""Configuration of Franka Emika Panda robot with stiffer PD control. + +This configuration is useful for task-space control using differential IK. +""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid.py new file mode 100644 index 00000000000..6ff52146c12 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Mujoco Humanoid robot.""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +HUMANOID_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=None, + max_depenetration_velocity=10.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.34), + joint_pos={".*": 0.0}, + ), + actuators={ + "body": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness={ + ".*_waist.*": 20.0, + ".*_upper_arm.*": 10.0, + "pelvis": 10.0, + ".*_lower_arm": 2.0, + ".*_thigh:0": 10.0, + ".*_thigh:1": 20.0, + ".*_thigh:2": 10.0, + ".*_shin": 5.0, + ".*_foot.*": 2.0, + }, + damping={ + ".*_waist.*": 5.0, + ".*_upper_arm.*": 5.0, + "pelvis": 5.0, + ".*_lower_arm": 1.0, + ".*_thigh:0": 5.0, + ".*_thigh:1": 5.0, + ".*_thigh:2": 5.0, + ".*_shin": 0.1, + ".*_foot.*": 1.0, + }, + ), + }, +) +"""Configuration for the Mujoco Humanoid robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid_28.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid_28.py new file mode 100644 index 00000000000..1af903babc6 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/humanoid_28.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the 28-DOFs Mujoco Humanoid robot.""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +HUMANOID_28_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Classic/Humanoid28/humanoid_28.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=None, + max_depenetration_velocity=10.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.8), + joint_pos={".*": 0.0}, + ), + actuators={ + "body": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the 28-DOFs Mujoco Humanoid robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/kinova.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/kinova.py new file mode 100644 index 00000000000..ff14f55cc37 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/kinova.py @@ -0,0 +1,182 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Kinova Robotics arms. + +The following configuration parameters are available: + +* :obj:`KINOVA_JACO2_N7S300_CFG`: The Kinova JACO2 (7-Dof) arm with a 3-finger gripper. +* :obj:`KINOVA_JACO2_N6S300_CFG`: The Kinova JACO2 (6-Dof) arm with a 3-finger gripper. +* :obj:`KINOVA_GEN3_N7_CFG`: The Kinova Gen3 (7-Dof) arm with no gripper. + +Reference: https://github.com/Kinovarobotics/kinova-ros +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +KINOVA_JACO2_N7S300_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "j2n7s300_joint_1": 0.0, + "j2n7s300_joint_2": 2.76, + "j2n7s300_joint_3": 0.0, + "j2n7s300_joint_4": 2.0, + "j2n7s300_joint_5": 2.0, + "j2n7s300_joint_6": 0.0, + "j2n7s300_joint_7": 0.0, + "j2n7s300_joint_finger_[1-3]": 0.2, # close: 1.2, open: 0.2 + "j2n7s300_joint_finger_tip_[1-3]": 0.2, # close: 1.2, open: 0.2 + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*_joint_[1-7]"], + velocity_limit=100.0, + effort_limit={ + ".*_joint_[1-2]": 80.0, + ".*_joint_[3-4]": 40.0, + ".*_joint_[5-7]": 20.0, + }, + stiffness={ + ".*_joint_[1-4]": 40.0, + ".*_joint_[5-7]": 15.0, + }, + damping={ + ".*_joint_[1-4]": 1.0, + ".*_joint_[5-7]": 0.5, + }, + ), + "gripper": ImplicitActuatorCfg( + joint_names_expr=[".*_finger_[1-3]", ".*_finger_tip_[1-3]"], + velocity_limit=100.0, + effort_limit=2.0, + stiffness=1.2, + damping=0.01, + ), + }, +) +"""Configuration of Kinova JACO2 (7-Dof) arm with 3-finger gripper.""" + + +KINOVA_JACO2_N6S300_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "j2n6s300_joint_1": 0.0, + "j2n6s300_joint_2": 2.76, + "j2n6s300_joint_3": 2.76, + "j2n6s300_joint_4": 2.5, + "j2n6s300_joint_5": 2.0, + "j2n6s300_joint_6": 0.0, + "j2n6s300_joint_finger_[1-3]": 0.2, # close: 1.2, open: 0.2 + "j2n6s300_joint_finger_tip_[1-3]": 0.2, # close: 1.2, open: 0.2 + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*_joint_[1-6]"], + velocity_limit=100.0, + effort_limit={ + ".*_joint_[1-2]": 80.0, + ".*_joint_3": 40.0, + ".*_joint_[4-6]": 20.0, + }, + stiffness={ + ".*_joint_[1-3]": 40.0, + ".*_joint_[4-6]": 15.0, + }, + damping={ + ".*_joint_[1-3]": 1.0, + ".*_joint_[4-6]": 0.5, + }, + ), + "gripper": ImplicitActuatorCfg( + joint_names_expr=[".*_finger_[1-3]", ".*_finger_tip_[1-3]"], + velocity_limit=100.0, + effort_limit=2.0, + stiffness=1.2, + damping=0.01, + ), + }, +) +"""Configuration of Kinova JACO2 (6-Dof) arm with 3-finger gripper.""" + + +KINOVA_GEN3_N7_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Kinova/Gen3/gen3n7_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "joint_1": 0.0, + "joint_2": 0.65, + "joint_3": 0.0, + "joint_4": 1.89, + "joint_5": 0.0, + "joint_6": 0.6, + "joint_7": -1.57, + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=["joint_[1-7]"], + velocity_limit=100.0, + effort_limit={ + "joint_[1-4]": 39.0, + "joint_[5-7]": 9.0, + }, + stiffness={ + "joint_[1-4]": 40.0, + "joint_[5-7]": 15.0, + }, + damping={ + "joint_[1-4]": 1.0, + "joint_[5-7]": 0.5, + }, + ), + }, +) +"""Configuration of Kinova Gen3 (7-Dof) arm with no gripper.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/quadcopter.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/quadcopter.py new file mode 100644 index 00000000000..1075b59ca0b --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/quadcopter.py @@ -0,0 +1,62 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the quadcopters""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +CRAZYFLIE_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Crazyflie/cf2x.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=10.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + joint_pos={ + ".*": 0.0, + }, + joint_vel={ + "m1_joint": 200.0, + "m2_joint": -200.0, + "m3_joint": 200.0, + "m4_joint": -200.0, + }, + ), + actuators={ + "dummy": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.0, + damping=0.0, + ), + }, +) +"""Configuration for the Crazyflie quadcopter.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ridgeback_franka.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ridgeback_franka.py new file mode 100644 index 00000000000..834636f4a56 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/ridgeback_franka.py @@ -0,0 +1,93 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Ridgeback-Manipulation robots. + +The following configurations are available: + +* :obj:`RIDGEBACK_FRANKA_PANDA_CFG`: Clearpath Ridgeback base with Franka Emika arm + +Reference: https://github.com/ridgeback/ridgeback_manipulation +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +RIDGEBACK_FRANKA_PANDA_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd", + articulation_props=sim_utils.ArticulationRootPropertiesCfg(enabled_self_collisions=False), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # base + "dummy_base_prismatic_y_joint": 0.0, + "dummy_base_prismatic_x_joint": 0.0, + "dummy_base_revolute_z_joint": 0.0, + # franka arm + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.810, + "panda_joint5": 0.0, + "panda_joint6": 2.0, + "panda_joint7": 0.741, + # tool + "panda_finger_joint.*": 0.035, + }, + joint_vel={".*": 0.0}, + ), + actuators={ + "base": ImplicitActuatorCfg( + joint_names_expr=["dummy_base_.*"], + velocity_limit=100.0, + effort_limit=1000.0, + stiffness=0.0, + damping=1e5, + ), + "panda_shoulder": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=800.0, + damping=40.0, + ), + "panda_forearm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + effort_limit=12.0, + velocity_limit=100.0, + stiffness=800.0, + damping=40.0, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint.*"], + effort_limit=200.0, + velocity_limit=0.2, + stiffness=1e5, + damping=1e3, + ), + }, +) +"""Configuration of Franka arm with Franka Hand on a Clearpath Ridgeback base using implicit actuator models. + +The following control configuration is used: + +* Base: velocity control +* Arm: position control with damping +* Hand: position control with damping + +""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/sawyer.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/sawyer.py new file mode 100644 index 00000000000..628cff2cb73 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/sawyer.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Rethink Robotics arms. + +The following configuration parameters are available: + +* :obj:`SAWYER_CFG`: The Sawyer arm without any tool attached. + +Reference: https://github.com/RethinkRobotics/sawyer_robot +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +SAWYER_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/RethinkRobotics/sawyer_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0 + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "head_pan": 0.0, + "right_j0": 0.0, + "right_j1": -0.785, + "right_j2": 0.0, + "right_j3": 1.05, + "right_j4": 0.0, + "right_j5": 1.3, + "right_j6": 0.0, + }, + ), + actuators={ + "head": ImplicitActuatorCfg( + joint_names_expr=["head_pan"], + velocity_limit=100.0, + effort_limit=8.0, + stiffness=800.0, + damping=40.0, + ), + "arm": ImplicitActuatorCfg( + joint_names_expr=["right_j[0-6]"], + velocity_limit=100.0, + effort_limit={ + "right_j[0-1]": 80.0, + "right_j[2-3]": 40.0, + "right_j[4-6]": 9.0, + }, + stiffness=100.0, + damping=4.0, + ), + }, +) +"""Configuration of Rethink Robotics Sawyer arm.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/shadow_hand.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/shadow_hand.py new file mode 100644 index 00000000000..e55317821fc --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/shadow_hand.py @@ -0,0 +1,90 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the dexterous hand from Shadow Robot. + +The following configurations are available: + +* :obj:`SHADOW_HAND_CFG`: Shadow Hand with implicit actuator model. + +Reference: + +* https://www.shadowrobot.com/dexterous-hand-series/ + +""" + + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +SHADOW_HAND_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ShadowHand/shadow_hand_instanceable.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=True, + max_depenetration_velocity=1000.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0005, + ), + # collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(drive_type="force"), + fixed_tendons_props=sim_utils.FixedTendonPropertiesCfg(limit_stiffness=30.0, damping=0.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(0.0, 0.0, -0.7071, 0.7071), + joint_pos={".*": 0.0}, + ), + actuators={ + "fingers": ImplicitActuatorCfg( + joint_names_expr=["robot0_WR.*", "robot0_(FF|MF|RF|LF|TH)J(3|2|1)", "robot0_(LF|TH)J4", "robot0_THJ0"], + effort_limit={ + "robot0_WRJ1": 4.785, + "robot0_WRJ0": 2.175, + "robot0_(FF|MF|RF|LF)J1": 0.7245, + "robot0_FFJ(3|2)": 0.9, + "robot0_MFJ(3|2)": 0.9, + "robot0_RFJ(3|2)": 0.9, + "robot0_LFJ(4|3|2)": 0.9, + "robot0_THJ4": 2.3722, + "robot0_THJ3": 1.45, + "robot0_THJ(2|1)": 0.99, + "robot0_THJ0": 0.81, + }, + stiffness={ + "robot0_WRJ.*": 5.0, + "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 1.0, + "robot0_(LF|TH)J4": 1.0, + "robot0_THJ0": 1.0, + }, + damping={ + "robot0_WRJ.*": 0.5, + "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 0.1, + "robot0_(LF|TH)J4": 0.1, + "robot0_THJ0": 0.1, + }, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of Shadow Hand robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/spot.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/spot.py new file mode 100644 index 00000000000..1da947f1789 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/spot.py @@ -0,0 +1,187 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Configuration for the Boston Dynamics robot. + +The following configuration parameters are available: + +* :obj:`SPOT_CFG`: The Spot robot with delay PD and remote PD actuators. +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import DelayedPDActuatorCfg, RemotizedPDActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +# Note: This data was collected by the Boston Dynamics AI Institute. +joint_parameter_lookup = [ + [-2.792900, -24.776718, 37.165077], + [-2.767442, -26.290108, 39.435162], + [-2.741984, -27.793369, 41.690054], + [-2.716526, -29.285997, 43.928996], + [-2.691068, -30.767536, 46.151304], + [-2.665610, -32.237423, 48.356134], + [-2.640152, -33.695168, 50.542751], + [-2.614694, -35.140221, 52.710331], + [-2.589236, -36.572052, 54.858078], + [-2.563778, -37.990086, 56.985128], + [-2.538320, -39.393730, 59.090595], + [-2.512862, -40.782406, 61.173609], + [-2.487404, -42.155487, 63.233231], + [-2.461946, -43.512371, 65.268557], + [-2.436488, -44.852371, 67.278557], + [-2.411030, -46.174873, 69.262310], + [-2.385572, -47.479156, 71.218735], + [-2.360114, -48.764549, 73.146824], + [-2.334656, -50.030334, 75.045502], + [-2.309198, -51.275761, 76.913641], + [-2.283740, -52.500103, 78.750154], + [-2.258282, -53.702587, 80.553881], + [-2.232824, -54.882442, 82.323664], + [-2.207366, -56.038860, 84.058290], + [-2.181908, -57.171028, 85.756542], + [-2.156450, -58.278133, 87.417200], + [-2.130992, -59.359314, 89.038971], + [-2.105534, -60.413738, 90.620607], + [-2.080076, -61.440529, 92.160793], + [-2.054618, -62.438812, 93.658218], + [-2.029160, -63.407692, 95.111538], + [-2.003702, -64.346268, 96.519402], + [-1.978244, -65.253670, 97.880505], + [-1.952786, -66.128944, 99.193417], + [-1.927328, -66.971176, 100.456764], + [-1.901870, -67.779457, 101.669186], + [-1.876412, -68.552864, 102.829296], + [-1.850954, -69.290451, 103.935677], + [-1.825496, -69.991325, 104.986988], + [-1.800038, -70.654541, 105.981812], + [-1.774580, -71.279190, 106.918785], + [-1.749122, -71.864319, 107.796478], + [-1.723664, -72.409088, 108.613632], + [-1.698206, -72.912567, 109.368851], + [-1.672748, -73.373871, 110.060806], + [-1.647290, -73.792130, 110.688194], + [-1.621832, -74.166512, 111.249767], + [-1.596374, -74.496147, 111.744221], + [-1.570916, -74.780251, 112.170376], + [-1.545458, -75.017998, 112.526997], + [-1.520000, -75.208656, 112.812984], + [-1.494542, -75.351448, 113.027172], + [-1.469084, -75.445686, 113.168530], + [-1.443626, -75.490677, 113.236015], + [-1.418168, -75.485771, 113.228657], + [-1.392710, -75.430344, 113.145515], + [-1.367252, -75.323830, 112.985744], + [-1.341794, -75.165688, 112.748531], + [-1.316336, -74.955406, 112.433109], + [-1.290878, -74.692551, 112.038826], + [-1.265420, -74.376694, 111.565041], + [-1.239962, -74.007477, 111.011215], + [-1.214504, -73.584579, 110.376869], + [-1.189046, -73.107742, 109.661613], + [-1.163588, -72.576752, 108.865128], + [-1.138130, -71.991455, 107.987183], + [-1.112672, -71.351707, 107.027561], + [-1.087214, -70.657486, 105.986229], + [-1.061756, -69.908813, 104.863220], + [-1.036298, -69.105721, 103.658581], + [-1.010840, -68.248337, 102.372505], + [-0.985382, -67.336861, 101.005291], + [-0.959924, -66.371513, 99.557270], + [-0.934466, -65.352615, 98.028923], + [-0.909008, -64.280533, 96.420799], + [-0.883550, -63.155693, 94.733540], + [-0.858092, -61.978588, 92.967882], + [-0.832634, -60.749775, 91.124662], + [-0.807176, -59.469845, 89.204767], + [-0.781718, -58.139503, 87.209255], + [-0.756260, -56.759487, 85.139231], + [-0.730802, -55.330616, 82.995924], + [-0.705344, -53.853729, 80.780594], + [-0.679886, -52.329796, 78.494694], + [-0.654428, -50.759762, 76.139643], + [-0.628970, -49.144699, 73.717049], + [-0.603512, -47.485737, 71.228605], + [-0.578054, -45.784004, 68.676006], + [-0.552596, -44.040764, 66.061146], + [-0.527138, -42.257267, 63.385900], + [-0.501680, -40.434883, 60.652325], + [-0.476222, -38.574947, 57.862421], + [-0.450764, -36.678982, 55.018473], + [-0.425306, -34.748432, 52.122648], + [-0.399848, -32.784836, 49.177254], + [-0.374390, -30.789810, 46.184715], + [-0.348932, -28.764952, 43.147428], + [-0.323474, -26.711969, 40.067954], + [-0.298016, -24.632576, 36.948864], + [-0.272558, -22.528547, 33.792821], + [-0.247100, -20.401667, 30.602500], +] +"""The lookup table for the knee joint parameters of the Boston Dynamics Spot robot. + +This table describes the relationship between the joint angle (rad), the transmission ratio (in/out), +and the output torque (N*m). It is used to interpolate the output torque based on the joint angle. +""" + +## +# Configuration +## + + +SPOT_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/BostonDynamics/spot/spot.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + joint_pos={ + "[fh]l_hx": 0.1, # all left hip_x + "[fh]r_hx": -0.1, # all right hip_x + "f[rl]_hy": 0.9, # front hip_y + "h[rl]_hy": 1.1, # hind hip_y + ".*_kn": -1.5, # all knees + }, + joint_vel={".*": 0.0}, + ), + actuators={ + "spot_hip": DelayedPDActuatorCfg( + joint_names_expr=[".*_h[xy]"], + effort_limit=45.0, + stiffness=60.0, + damping=1.5, + min_delay=0, # physics time steps (min: 2.0*0=0.0ms) + max_delay=4, # physics time steps (max: 2.0*4=8.0ms) + ), + "spot_knee": RemotizedPDActuatorCfg( + joint_names_expr=[".*_kn"], + joint_parameter_lookup=joint_parameter_lookup, + effort_limit=None, # torque limits are handled based experimental data (`RemotizedPDActuatorCfg.data`) + stiffness=60.0, + damping=1.5, + min_delay=0, # physics time steps (min: 2.0*0=0.0ms) + max_delay=4, # physics time steps (max: 2.0*4=8.0ms) + ), + }, +) +"""Configuration for the Boston Dynamics Spot robot.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/unitree.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/unitree.py new file mode 100644 index 00000000000..a336f9b72e3 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/unitree.py @@ -0,0 +1,393 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Unitree robots. + +The following configurations are available: + +* :obj:`UNITREE_A1_CFG`: Unitree A1 robot with DC motor model for the legs +* :obj:`UNITREE_GO1_CFG`: Unitree Go1 robot with actuator net model for the legs +* :obj:`UNITREE_GO2_CFG`: Unitree Go2 robot with DC motor model for the legs +* :obj:`H1_CFG`: H1 humanoid robot +* :obj:`H1_MINIMAL_CFG`: H1 humanoid robot with minimal collision bodies +* :obj:`G1_CFG`: G1 humanoid robot +* :obj:`G1_MINIMAL_CFG`: G1 humanoid robot with minimal collision bodies + +Reference: https://github.com/unitreerobotics/unitree_ros +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration - Actuators. +## + +GO1_ACTUATOR_CFG = ActuatorNetMLPCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + network_file=f"{ISAACLAB_NUCLEUS_DIR}/ActuatorNets/Unitree/unitree_go1.pt", + pos_scale=-1.0, + vel_scale=1.0, + torque_scale=1.0, + input_order="pos_vel", + input_idx=[0, 1, 2], + effort_limit=23.7, # taken from spec sheet + velocity_limit=30.0, # taken from spec sheet + saturation_effort=23.7, # same as effort limit +) +"""Configuration of Go1 actuators using MLP model. + +Actuator specifications: https://shop.unitree.com/products/go1-motor + +This model is taken from: https://github.com/Improbable-AI/walk-these-ways +""" + + +## +# Configuration +## + + +UNITREE_A1_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/A1/a1.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.42), + joint_pos={ + ".*L_hip_joint": 0.1, + ".*R_hip_joint": -0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.5, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "base_legs": DCMotorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit=33.5, + saturation_effort=33.5, + velocity_limit=21.0, + stiffness=25.0, + damping=0.5, + friction=0.0, + ), + }, +) +"""Configuration of Unitree A1 using DC motor. + +Note: Specifications taken from: https://www.trossenrobotics.com/a1-quadruped#specifications +""" + + +UNITREE_GO1_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/Go1/go1.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.4), + joint_pos={ + ".*L_hip_joint": 0.1, + ".*R_hip_joint": -0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.5, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "base_legs": GO1_ACTUATOR_CFG, + }, +) +"""Configuration of Unitree Go1 using MLP-based actuator model.""" + + +UNITREE_GO2_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/Go2/go2.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.4), + joint_pos={ + ".*L_hip_joint": 0.1, + ".*R_hip_joint": -0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.5, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "base_legs": DCMotorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit=23.5, + saturation_effort=23.5, + velocity_limit=30.0, + stiffness=25.0, + damping=0.5, + friction=0.0, + ), + }, +) +"""Configuration of Unitree Go2 using DC-Motor actuator model.""" + + +H1_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/H1/h1.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.05), + joint_pos={ + ".*_hip_yaw": 0.0, + ".*_hip_roll": 0.0, + ".*_hip_pitch": -0.28, # -16 degrees + ".*_knee": 0.79, # 45 degrees + ".*_ankle": -0.52, # -30 degrees + "torso": 0.0, + ".*_shoulder_pitch": 0.28, + ".*_shoulder_roll": 0.0, + ".*_shoulder_yaw": 0.0, + ".*_elbow": 0.52, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw", ".*_hip_roll", ".*_hip_pitch", ".*_knee", "torso"], + effort_limit=300, + velocity_limit=100.0, + stiffness={ + ".*_hip_yaw": 150.0, + ".*_hip_roll": 150.0, + ".*_hip_pitch": 200.0, + ".*_knee": 200.0, + "torso": 200.0, + }, + damping={ + ".*_hip_yaw": 5.0, + ".*_hip_roll": 5.0, + ".*_hip_pitch": 5.0, + ".*_knee": 5.0, + "torso": 5.0, + }, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle"], + effort_limit=100, + velocity_limit=100.0, + stiffness={".*_ankle": 20.0}, + damping={".*_ankle": 4.0}, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[".*_shoulder_pitch", ".*_shoulder_roll", ".*_shoulder_yaw", ".*_elbow"], + effort_limit=300, + velocity_limit=100.0, + stiffness={ + ".*_shoulder_pitch": 40.0, + ".*_shoulder_roll": 40.0, + ".*_shoulder_yaw": 40.0, + ".*_elbow": 40.0, + }, + damping={ + ".*_shoulder_pitch": 10.0, + ".*_shoulder_roll": 10.0, + ".*_shoulder_yaw": 10.0, + ".*_elbow": 10.0, + }, + ), + }, +) +"""Configuration for the Unitree H1 Humanoid robot.""" + + +H1_MINIMAL_CFG = H1_CFG.copy() +H1_MINIMAL_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/H1/h1_minimal.usd" +"""Configuration for the Unitree H1 Humanoid robot with fewer collision meshes. + +This configuration removes most collision meshes to speed up simulation. +""" + + +G1_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.74), + joint_pos={ + ".*_hip_pitch_joint": -0.20, + ".*_knee_joint": 0.42, + ".*_ankle_pitch_joint": -0.23, + ".*_elbow_pitch_joint": 0.87, + "left_shoulder_roll_joint": 0.16, + "left_shoulder_pitch_joint": 0.35, + "right_shoulder_roll_joint": -0.16, + "right_shoulder_pitch_joint": 0.35, + "left_one_joint": 1.0, + "right_one_joint": -1.0, + "left_two_joint": 0.52, + "right_two_joint": -0.52, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_hip_yaw_joint", + ".*_hip_roll_joint", + ".*_hip_pitch_joint", + ".*_knee_joint", + "torso_joint", + ], + effort_limit=300, + velocity_limit=100.0, + stiffness={ + ".*_hip_yaw_joint": 150.0, + ".*_hip_roll_joint": 150.0, + ".*_hip_pitch_joint": 200.0, + ".*_knee_joint": 200.0, + "torso_joint": 200.0, + }, + damping={ + ".*_hip_yaw_joint": 5.0, + ".*_hip_roll_joint": 5.0, + ".*_hip_pitch_joint": 5.0, + ".*_knee_joint": 5.0, + "torso_joint": 5.0, + }, + armature={ + ".*_hip_.*": 0.01, + ".*_knee_joint": 0.01, + "torso_joint": 0.01, + }, + ), + "feet": ImplicitActuatorCfg( + effort_limit=20, + joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], + stiffness=20.0, + damping=2.0, + armature=0.01, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_pitch_joint", + ".*_elbow_roll_joint", + ".*_five_joint", + ".*_three_joint", + ".*_six_joint", + ".*_four_joint", + ".*_zero_joint", + ".*_one_joint", + ".*_two_joint", + ], + effort_limit=300, + velocity_limit=100.0, + stiffness=40.0, + damping=10.0, + armature={ + ".*_shoulder_.*": 0.01, + ".*_elbow_.*": 0.01, + ".*_five_joint": 0.001, + ".*_three_joint": 0.001, + ".*_six_joint": 0.001, + ".*_four_joint": 0.001, + ".*_zero_joint": 0.001, + ".*_one_joint": 0.001, + ".*_two_joint": 0.001, + }, + ), + }, +) +"""Configuration for the Unitree G1 Humanoid robot.""" + + +G1_MINIMAL_CFG = G1_CFG.copy() +G1_MINIMAL_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1_minimal.usd" +"""Configuration for the Unitree G1 Humanoid robot with fewer collision meshes. + +This configuration removes most collision meshes to speed up simulation. +""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/universal_robots.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/universal_robots.py new file mode 100644 index 00000000000..429c209910d --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/robots/universal_robots.py @@ -0,0 +1,59 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Universal Robots. + +The following configuration parameters are available: + +* :obj:`UR10_CFG`: The UR10 arm without a gripper. + +Reference: https://github.com/ros-industrial/universal_robot +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + + +UR10_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -1.712, + "elbow_joint": 1.712, + "wrist_1_joint": 0.0, + "wrist_2_joint": 0.0, + "wrist_3_joint": 0.0, + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*"], + velocity_limit=100.0, + effort_limit=87.0, + stiffness=800.0, + damping=40.0, + ), + }, +) +"""Configuration of UR-10 arm using implicit actuator models.""" diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/__init__.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/__init__.py new file mode 100644 index 00000000000..c46ca934b35 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +## +# Configuration for different assets. +## + +from .velodyne import * diff --git a/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/velodyne.py b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/velodyne.py new file mode 100644 index 00000000000..7f73fd22619 --- /dev/null +++ b/install/isaaclab_assets/lib/python3.10/site-packages/isaaclab_assets/sensors/velodyne.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Velodyne LiDAR sensors.""" + + +from isaaclab.sensors import RayCasterCfg, patterns + +## +# Configuration +## + +VELODYNE_VLP_16_RAYCASTER_CFG = RayCasterCfg( + attach_yaw_only=False, + pattern_cfg=patterns.LidarPatternCfg( + channels=16, vertical_fov_range=(-15.0, 15.0), horizontal_fov_range=(-180.0, 180.0), horizontal_res=0.2 + ), + debug_vis=True, + max_distance=100, +) +"""Configuration for Velodyne Puck LiDAR (VLP-16) as a :class:`RayCasterCfg`. + +Reference: https://velodynelidar.com/wp-content/uploads/2019/12/63-9229_Rev-K_Puck-_Datasheet_Web.pdf +""" diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/__init__.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/__init__.py new file mode 100644 index 00000000000..a60cdf6884e --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Package containing implementation of Isaac Lab Mimic data generation.""" + +__version__ = "1.0.0" diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/__init__.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/__init__.py new file mode 100644 index 00000000000..519365a47a7 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with core implementation logic for Isaac Lab Mimic.""" + +from .data_generator import * +from .datagen_info import * +from .datagen_info_pool import * +from .generation import * +from .selection_strategy import * +from .utils import * +from .waypoint import * diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/data_generator.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/data_generator.py new file mode 100644 index 00000000000..adc137a150e --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/data_generator.py @@ -0,0 +1,788 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +""" +Base class for data generator. +""" +import asyncio +import numpy as np +import torch + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ( + ManagerBasedRLMimicEnv, + MimicEnvCfg, + SubTaskConstraintCoordinationScheme, + SubTaskConstraintType, +) +from isaaclab.managers import TerminationTermCfg + +from isaaclab_mimic.datagen.datagen_info import DatagenInfo +from isaaclab_mimic.datagen.selection_strategy import make_selection_strategy +from isaaclab_mimic.datagen.waypoint import MultiWaypoint, Waypoint, WaypointSequence, WaypointTrajectory + +from .datagen_info_pool import DataGenInfoPool + + +def transform_source_data_segment_using_delta_object_pose( + src_eef_poses: torch.Tensor, + delta_obj_pose: torch.Tensor, +) -> torch.Tensor: + """ + Transform a source data segment (object-centric subtask segment from source demonstration) using + a delta object pose. + + Args: + src_eef_poses: pose sequence (shape [T, 4, 4]) for the sequence of end effector control poses + from the source demonstration + delta_obj_pose: 4x4 delta object pose + + Returns: + transformed_eef_poses: transformed pose sequence (shape [T, 4, 4]) + """ + return PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_poses, + pose_A_in_B=delta_obj_pose[None], + ) + + +def transform_source_data_segment_using_object_pose( + obj_pose: torch.Tensor, + src_eef_poses: torch.Tensor, + src_obj_pose: torch.Tensor, +) -> torch.Tensor: + """ + Transform a source data segment (object-centric subtask segment from source demonstration) such that + the relative poses between the target eef pose frame and the object frame are preserved. Recall that + each object-centric subtask segment corresponds to one object, and consists of a sequence of + target eef poses. + + Args: + obj_pose: 4x4 object pose in current scene + src_eef_poses: pose sequence (shape [T, 4, 4]) for the sequence of end effector control poses + from the source demonstration + src_obj_pose: 4x4 object pose from the source demonstration + + Returns: + transformed_eef_poses: transformed pose sequence (shape [T, 4, 4]) + """ + + # transform source end effector poses to be relative to source object frame + src_eef_poses_rel_obj = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_poses, + pose_A_in_B=PoseUtils.pose_inv(src_obj_pose[None]), + ) + + # apply relative poses to current object frame to obtain new target eef poses + transformed_eef_poses = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_poses_rel_obj, + pose_A_in_B=obj_pose[None], + ) + return transformed_eef_poses + + +def get_delta_pose_with_scheme( + src_obj_pose: torch.Tensor, + cur_obj_pose: torch.Tensor, + task_constraint: dict, +) -> torch.Tensor: + """ + Get the delta pose with the given coordination scheme. + + Args: + src_obj_pose: 4x4 object pose in source scene + cur_obj_pose: 4x4 object pose in current scene + task_constraint: task constraint dictionary + + Returns: + delta_pose: 4x4 delta pose + """ + coord_transform_scheme = task_constraint["coordination_scheme"] + device = src_obj_pose.device + if coord_transform_scheme == SubTaskConstraintCoordinationScheme.TRANSFORM: + delta_pose = PoseUtils.get_delta_object_pose(cur_obj_pose, src_obj_pose) + # add noise to delta pose position + elif coord_transform_scheme == SubTaskConstraintCoordinationScheme.TRANSLATE: + delta_pose = torch.eye(4, device=device) + delta_pose[:3, 3] = cur_obj_pose[:3, 3] - src_obj_pose[:3, 3] + elif coord_transform_scheme == SubTaskConstraintCoordinationScheme.REPLAY: + delta_pose = torch.eye(4, device=device) + else: + raise ValueError( + f"coordination coord_transform_scheme {coord_transform_scheme} not supported, only" + f" {[e.value for e in SubTaskConstraintCoordinationScheme]} are supported" + ) + + pos_noise_scale = task_constraint["coordination_scheme_pos_noise_scale"] + rot_noise_scale = task_constraint["coordination_scheme_rot_noise_scale"] + if pos_noise_scale != 0.0 or rot_noise_scale != 0.0: + pos = delta_pose[:3, 3] + rot = delta_pose[:3, :3] + pos_new, rot_new = PoseUtils.add_uniform_noise_to_pose(pos, rot, pos_noise_scale, rot_noise_scale) + delta_pose = torch.eye(4, device=device) + delta_pose[:3, 3] = pos_new + delta_pose[:3, :3] = rot_new + return delta_pose + + +class DataGenerator: + """ + The main data generator class that generates new trajectories from source datasets. + + The data generator, inspired by the MimicGen, enables the generation of new datasets based on a few human + collected source demonstrations. + + The data generator works by parsing demonstrations into object-centric subtask segments, stored in DataGenInfoPool. + It then adapts these subtask segments to new scenes by transforming each segment according to the new scene’s context, + stitching them into a coherent trajectory for a robotic end-effector to execute. + """ + + def __init__( + self, + env: ManagerBasedRLMimicEnv, + src_demo_datagen_info_pool: DataGenInfoPool | None = None, + dataset_path: str | None = None, + demo_keys: list[str] | None = None, + ): + """ + Args: + env: environment to use for data generation + src_demo_datagen_info_pool: source demo datagen info pool + dataset_path: path to hdf5 dataset to use for generation + demo_keys: list of demonstration keys to use in file. If not provided, all demonstration keys + will be used. + """ + self.env = env + self.env_cfg = env.cfg + assert isinstance(self.env_cfg, MimicEnvCfg) + self.dataset_path = dataset_path + + # sanity check on task spec offset ranges - final subtask should not have any offset randomization + for subtask_configs in self.env_cfg.subtask_configs.values(): + assert subtask_configs[-1].subtask_term_offset_range[0] == 0 + assert subtask_configs[-1].subtask_term_offset_range[1] == 0 + + self.demo_keys = demo_keys + + if src_demo_datagen_info_pool is not None: + self.src_demo_datagen_info_pool = src_demo_datagen_info_pool + elif dataset_path is not None: + self.src_demo_datagen_info_pool = DataGenInfoPool( + env=self.env, env_cfg=self.env_cfg, device=self.env.device + ) + self.src_demo_datagen_info_pool.load_from_dataset_file(dataset_path, select_demo_keys=self.demo_keys) + else: + raise ValueError("Either src_demo_datagen_info_pool or dataset_path must be provided") + + def __repr__(self): + """ + Pretty print this object. + """ + msg = str(self.__class__.__name__) + msg += " (\n\tdataset_path={}\n\tdemo_keys={}\n)".format( + self.dataset_path, + self.demo_keys, + ) + return msg + + def randomize_subtask_boundaries(self) -> dict[str, np.ndarray]: + """ + Apply random offsets to sample subtask boundaries according to the task spec. + Recall that each demonstration is segmented into a set of subtask segments, and the + end index of each subtask can have a random offset. + """ + + randomized_subtask_boundaries = {} + + for eef_name, subtask_boundaries in self.src_demo_datagen_info_pool.subtask_boundaries.items(): + # initial subtask start and end indices - shape (N, S, 2) + subtask_boundaries = np.array(subtask_boundaries) + + # Randomize the start of the first subtask + first_subtask_start_offsets = np.random.randint( + low=self.env_cfg.subtask_configs[eef_name][0].first_subtask_start_offset_range[0], + high=self.env_cfg.subtask_configs[eef_name][0].first_subtask_start_offset_range[0] + 1, + size=subtask_boundaries.shape[0], + ) + subtask_boundaries[:, 0, 0] += first_subtask_start_offsets + + # for each subtask (except last one), sample all end offsets at once for each demonstration + # add them to subtask end indices, and then set them as the start indices of next subtask too + for i in range(subtask_boundaries.shape[1] - 1): + end_offsets = np.random.randint( + low=self.env_cfg.subtask_configs[eef_name][i].subtask_term_offset_range[0], + high=self.env_cfg.subtask_configs[eef_name][i].subtask_term_offset_range[1] + 1, + size=subtask_boundaries.shape[0], + ) + subtask_boundaries[:, i, 1] = subtask_boundaries[:, i, 1] + end_offsets + # don't forget to set these as start indices for next subtask too + subtask_boundaries[:, i + 1, 0] = subtask_boundaries[:, i, 1] + + # ensure non-empty subtasks + assert np.all((subtask_boundaries[:, :, 1] - subtask_boundaries[:, :, 0]) > 0), "got empty subtasks!" + + # ensure subtask indices increase (both starts and ends) + assert np.all( + (subtask_boundaries[:, 1:, :] - subtask_boundaries[:, :-1, :]) > 0 + ), "subtask indices do not strictly increase" + + # ensure subtasks are in order + subtask_inds_flat = subtask_boundaries.reshape(subtask_boundaries.shape[0], -1) + assert np.all((subtask_inds_flat[:, 1:] - subtask_inds_flat[:, :-1]) >= 0), "subtask indices not in order" + + randomized_subtask_boundaries[eef_name] = subtask_boundaries + + return randomized_subtask_boundaries + + def select_source_demo( + self, + eef_name: str, + eef_pose: np.ndarray, + object_pose: np.ndarray, + src_demo_current_subtask_boundaries: np.ndarray, + subtask_object_name: str, + selection_strategy_name: str, + selection_strategy_kwargs: dict | None = None, + ) -> int: + """ + Helper method to run source subtask segment selection. + + Args: + eef_name: name of end effector + eef_pose: current end effector pose + object_pose: current object pose for this subtask + src_demo_current_subtask_boundaries: start and end indices for subtask segment in source demonstrations of shape (N, 2) + subtask_object_name: name of reference object for this subtask + selection_strategy_name: name of selection strategy + selection_strategy_kwargs: extra kwargs for running selection strategy + + Returns: + selected_src_demo_ind: selected source demo index + """ + if subtask_object_name is None: + # no reference object - only random selection is supported + assert selection_strategy_name == "random", selection_strategy_name + + # We need to collect the datagen info objects over the timesteps for the subtask segment in each source + # demo, so that it can be used by the selection strategy. + src_subtask_datagen_infos = [] + for i in range(len(self.src_demo_datagen_info_pool.datagen_infos)): + # datagen info over all timesteps of the src trajectory + src_ep_datagen_info = self.src_demo_datagen_info_pool.datagen_infos[i] + + # time indices for subtask + subtask_start_ind = src_demo_current_subtask_boundaries[i][0] + subtask_end_ind = src_demo_current_subtask_boundaries[i][1] + + # get subtask segment using indices + src_subtask_datagen_infos.append( + DatagenInfo( + eef_pose=src_ep_datagen_info.eef_pose[eef_name][subtask_start_ind:subtask_end_ind], + # only include object pose for relevant object in subtask + object_poses=( + { + subtask_object_name: src_ep_datagen_info.object_poses[subtask_object_name][ + subtask_start_ind:subtask_end_ind + ] + } + if (subtask_object_name is not None) + else None + ), + # subtask termination signal is unused + subtask_term_signals=None, + target_eef_pose=src_ep_datagen_info.target_eef_pose[eef_name][subtask_start_ind:subtask_end_ind], + gripper_action=src_ep_datagen_info.gripper_action[eef_name][subtask_start_ind:subtask_end_ind], + ) + ) + + # make selection strategy object + selection_strategy_obj = make_selection_strategy(selection_strategy_name) + + # run selection + if selection_strategy_kwargs is None: + selection_strategy_kwargs = dict() + selected_src_demo_ind = selection_strategy_obj.select_source_demo( + eef_pose=eef_pose, + object_pose=object_pose, + src_subtask_datagen_infos=src_subtask_datagen_infos, + **selection_strategy_kwargs, + ) + + return selected_src_demo_ind + + def generate_trajectory( + self, + env_id: int, + eef_name: str, + subtask_ind: int, + all_randomized_subtask_boundaries: dict[str, np.ndarray], + runtime_subtask_constraints_dict: dict[tuple[str, int], dict], + selected_src_demo_inds: dict[str, int | None], + prev_executed_traj: dict[str, list[Waypoint] | None], + ) -> list[Waypoint]: + """ + Generate a trajectory for the given subtask. + + Args: + env_id: environment index + eef_name: name of end effector + subtask_ind: index of subtask + all_randomized_subtask_boundaries: randomized subtask boundaries + runtime_subtask_constraints_dict: runtime subtask constraints + selected_src_demo_inds: dictionary of selected source demo indices per eef, updated in place + prev_executed_traj: dictionary of previously executed eef trajectories + + Returns: + trajectory: generated trajectory + """ + subtask_configs = self.env_cfg.subtask_configs[eef_name] + # name of object for this subtask + subtask_object_name = self.env_cfg.subtask_configs[eef_name][subtask_ind].object_ref + subtask_object_pose = ( + self.env.get_object_poses(env_ids=[env_id])[subtask_object_name][0] + if (subtask_object_name is not None) + else None + ) + + is_first_subtask = subtask_ind == 0 + + need_source_demo_selection = is_first_subtask or self.env_cfg.datagen_config.generation_select_src_per_subtask + + if not self.env_cfg.datagen_config.generation_select_src_per_arm: + need_source_demo_selection = need_source_demo_selection and selected_src_demo_inds[eef_name] is None + + use_delta_transform = None + coord_transform_scheme = None + if (eef_name, subtask_ind) in runtime_subtask_constraints_dict: + if runtime_subtask_constraints_dict[(eef_name, subtask_ind)]["type"] == SubTaskConstraintType.COORDINATION: + # avoid selecting source demo if it has already been selected by the concurrent task + concurrent_task_spec_key = runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "concurrent_task_spec_key" + ] + concurrent_subtask_ind = runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "concurrent_subtask_ind" + ] + concurrent_selected_src_ind = runtime_subtask_constraints_dict[ + (concurrent_task_spec_key, concurrent_subtask_ind) + ]["selected_src_demo_ind"] + if concurrent_selected_src_ind is not None: + # the concurrent task has started, so we should use the same source demo + selected_src_demo_inds[eef_name] = concurrent_selected_src_ind + need_source_demo_selection = False + use_delta_transform = runtime_subtask_constraints_dict[ + (concurrent_task_spec_key, concurrent_subtask_ind) + ]["transform"] + else: + assert ( + "transform" not in runtime_subtask_constraints_dict[(eef_name, subtask_ind)] + ), "transform should not be set for concurrent task" + # need to transform demo according to scheme + coord_transform_scheme = runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "coordination_scheme" + ] + if coord_transform_scheme != SubTaskConstraintCoordinationScheme.REPLAY: + assert ( + subtask_object_name is not None + ), f"object reference should not be None for {coord_transform_scheme} coordination scheme" + + if need_source_demo_selection: + selected_src_demo_inds[eef_name] = self.select_source_demo( + eef_name=eef_name, + eef_pose=self.env.get_robot_eef_pose(env_ids=[env_id], eef_name=eef_name)[0], + object_pose=subtask_object_pose, + src_demo_current_subtask_boundaries=all_randomized_subtask_boundaries[eef_name][:, subtask_ind], + subtask_object_name=subtask_object_name, + selection_strategy_name=self.env_cfg.subtask_configs[eef_name][subtask_ind].selection_strategy, + selection_strategy_kwargs=self.env_cfg.subtask_configs[eef_name][subtask_ind].selection_strategy_kwargs, + ) + + assert selected_src_demo_inds[eef_name] is not None + selected_src_demo_ind = selected_src_demo_inds[eef_name] + + if not self.env_cfg.datagen_config.generation_select_src_per_arm and need_source_demo_selection: + for itrated_eef_name in self.env_cfg.subtask_configs.keys(): + selected_src_demo_inds[itrated_eef_name] = selected_src_demo_ind + + # selected subtask segment time indices + selected_src_subtask_boundary = all_randomized_subtask_boundaries[eef_name][selected_src_demo_ind, subtask_ind] + + if (eef_name, subtask_ind) in runtime_subtask_constraints_dict: + if runtime_subtask_constraints_dict[(eef_name, subtask_ind)]["type"] == SubTaskConstraintType.COORDINATION: + # store selected source demo ind for concurrent task + runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "selected_src_demo_ind" + ] = selected_src_demo_ind + concurrent_task_spec_key = runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "concurrent_task_spec_key" + ] + concurrent_subtask_ind = runtime_subtask_constraints_dict[(eef_name, subtask_ind)][ + "concurrent_subtask_ind" + ] + concurrent_src_subtask_inds = all_randomized_subtask_boundaries[concurrent_task_spec_key][ + selected_src_demo_ind, concurrent_subtask_ind + ] + subtask_len = selected_src_subtask_boundary[1] - selected_src_subtask_boundary[0] + concurrent_subtask_len = concurrent_src_subtask_inds[1] - concurrent_src_subtask_inds[0] + runtime_subtask_constraints_dict[(eef_name, subtask_ind)]["synchronous_steps"] = min( + subtask_len, concurrent_subtask_len + ) + + # TODO allow for different anchor selection strategies for each subtask + + # get subtask segment, consisting of the sequence of robot eef poses, target poses, gripper actions + src_ep_datagen_info = self.src_demo_datagen_info_pool.datagen_infos[selected_src_demo_ind] + src_subtask_eef_poses = src_ep_datagen_info.eef_pose[eef_name][ + selected_src_subtask_boundary[0] : selected_src_subtask_boundary[1] + ] + src_subtask_target_poses = src_ep_datagen_info.target_eef_pose[eef_name][ + selected_src_subtask_boundary[0] : selected_src_subtask_boundary[1] + ] + src_subtask_gripper_actions = src_ep_datagen_info.gripper_action[eef_name][ + selected_src_subtask_boundary[0] : selected_src_subtask_boundary[1] + ] + + # get reference object pose from source demo + src_subtask_object_pose = ( + src_ep_datagen_info.object_poses[subtask_object_name][selected_src_subtask_boundary[0]] + if (subtask_object_name is not None) + else None + ) + + if is_first_subtask or self.env_cfg.datagen_config.generation_transform_first_robot_pose: + # Source segment consists of first robot eef pose and the target poses. This ensures that + # we will interpolate to the first robot eef pose in this source segment, instead of the + # first robot target pose. + src_eef_poses = torch.cat([src_subtask_eef_poses[0:1], src_subtask_target_poses], dim=0) + # account for extra timestep added to @src_eef_poses + src_subtask_gripper_actions = torch.cat( + [src_subtask_gripper_actions[0:1], src_subtask_gripper_actions], dim=0 + ) + else: + # Source segment consists of just the target poses. + src_eef_poses = src_subtask_target_poses.clone() + src_subtask_gripper_actions = src_subtask_gripper_actions.clone() + + # Transform source demonstration segment using relevant object pose. + if use_delta_transform is not None: + # use delta transform from concurrent task + transformed_eef_poses = transform_source_data_segment_using_delta_object_pose( + src_eef_poses, use_delta_transform + ) + + # TODO: Uncomment below to support case of temporal concurrent but NOT does not require coordination. Need to decide if this case is necessary + # if subtask_object_name is not None: + # transformed_eef_poses = PoseUtils.transform_source_data_segment_using_object_pose( + # cur_object_poses[task_spec_idx], + # src_eef_poses, + # src_subtask_object_pose, + # ) + + else: + if coord_transform_scheme is not None: + delta_obj_pose = get_delta_pose_with_scheme( + src_subtask_object_pose, + subtask_object_pose, + runtime_subtask_constraints_dict[(eef_name, subtask_ind)], + ) + transformed_eef_poses = transform_source_data_segment_using_delta_object_pose( + src_eef_poses, delta_obj_pose + ) + runtime_subtask_constraints_dict[(eef_name, subtask_ind)]["transform"] = delta_obj_pose + else: + if subtask_object_name is not None: + transformed_eef_poses = transform_source_data_segment_using_object_pose( + subtask_object_pose, + src_eef_poses, + src_subtask_object_pose, + ) + else: + print(f"skipping transformation for {subtask_object_name}") + # skip transformation if no reference object is provided + transformed_eef_poses = src_eef_poses + + # We will construct a WaypointTrajectory instance to keep track of robot control targets + # that will be executed and then execute it. + traj_to_execute = WaypointTrajectory() + + if self.env_cfg.datagen_config.generation_interpolate_from_last_target_pose and (not is_first_subtask): + # Interpolation segment will start from last target pose (which may not have been achieved). + assert prev_executed_traj[eef_name] is not None + last_waypoint = prev_executed_traj[eef_name][-1] + init_sequence = WaypointSequence(sequence=[last_waypoint]) + else: + # Interpolation segment will start from current robot eef pose. + init_sequence = WaypointSequence.from_poses( + poses=self.env.get_robot_eef_pose(env_ids=[env_id], eef_name=eef_name)[0].unsqueeze(0), + gripper_actions=src_subtask_gripper_actions[0].unsqueeze(0), + action_noise=subtask_configs[subtask_ind].action_noise, + ) + traj_to_execute.add_waypoint_sequence(init_sequence) + + # Construct trajectory for the transformed segment. + transformed_seq = WaypointSequence.from_poses( + poses=transformed_eef_poses, + gripper_actions=src_subtask_gripper_actions, + action_noise=subtask_configs[subtask_ind].action_noise, + ) + transformed_traj = WaypointTrajectory() + transformed_traj.add_waypoint_sequence(transformed_seq) + + # Merge this trajectory into our trajectory using linear interpolation. + # Interpolation will happen from the initial pose (@init_sequence) to the first element of @transformed_seq. + traj_to_execute.merge( + transformed_traj, + num_steps_interp=self.env_cfg.subtask_configs[eef_name][subtask_ind].num_interpolation_steps, + num_steps_fixed=self.env_cfg.subtask_configs[eef_name][subtask_ind].num_fixed_steps, + action_noise=( + float(self.env_cfg.subtask_configs[eef_name][subtask_ind].apply_noise_during_interpolation) + * self.env_cfg.subtask_configs[eef_name][subtask_ind].action_noise + ), + ) + + # We initialized @traj_to_execute with a pose to allow @merge to handle linear interpolation + # for us. However, we can safely discard that first waypoint now, and just start by executing + # the rest of the trajectory (interpolation segment and transformed subtask segment). + traj_to_execute.pop_first() + + # Return the generated trajectory + return traj_to_execute.get_full_sequence().sequence + + async def generate( + self, + env_id: int, + success_term: TerminationTermCfg, + env_reset_queue: asyncio.Queue | None = None, + env_action_queue: asyncio.Queue | None = None, + pause_subtask: bool = False, + export_demo: bool = True, + ) -> dict: + """ + Attempt to generate a new demonstration. + + Args: + env_id: environment index + success_term: success function to check if the task is successful + env_reset_queue: queue to store environment IDs for reset + env_action_queue: queue to store actions for each environment + pause_subtask: if True, pause after every subtask during generation, for debugging + export_demo: if True, export the generated demonstration + + Returns: + results: dictionary with the following items: + initial_state (dict): initial simulator state for the executed trajectory + states (list): simulator state at each timestep + observations (list): observation dictionary at each timestep + datagen_infos (list): datagen_info at each timestep + actions (np.array): action executed at each timestep + success (bool): whether the trajectory successfully solved the task or not + src_demo_inds (list): list of selected source demonstration indices for each subtask + src_demo_labels (np.array): same as @src_demo_inds, but repeated to have a label for each timestep of the trajectory + """ + + # reset the env to create a new task demo instance + env_id_tensor = torch.tensor([env_id], dtype=torch.int64, device=self.env.device) + self.env.recorder_manager.reset(env_ids=env_id_tensor) + await env_reset_queue.put(env_id) + await env_reset_queue.join() + new_initial_state = self.env.scene.get_state(is_relative=True) + + # create runtime subtask constraint rules from subtask constraint configs + runtime_subtask_constraints_dict = {} + for subtask_constraint in self.env_cfg.task_constraint_configs: + runtime_subtask_constraints_dict.update(subtask_constraint.generate_runtime_subtask_constraints()) + + # save generated data in these variables + generated_states = [] + generated_obs = [] + generated_actions = [] + generated_success = False + + # some eef-specific state variables used during generation + current_eef_selected_src_demo_indices = {} + current_eef_subtask_trajectories = {} + current_eef_subtask_indices = {} + current_eef_subtask_step_indices = {} + eef_subtasks_done = {} + for eef_name in self.env_cfg.subtask_configs.keys(): + current_eef_selected_src_demo_indices[eef_name] = None + # prev_eef_executed_traj[eef_name] = None # type of list of Waypoint + current_eef_subtask_trajectories[eef_name] = None # type of list of Waypoint + current_eef_subtask_indices[eef_name] = 0 + current_eef_subtask_step_indices[eef_name] = None + eef_subtasks_done[eef_name] = False + + prev_src_demo_datagen_info_pool_size = 0 + # While loop that runs per time step + while True: + async with self.src_demo_datagen_info_pool.asyncio_lock: + if len(self.src_demo_datagen_info_pool.datagen_infos) > prev_src_demo_datagen_info_pool_size: + # src_demo_datagen_info_pool at this point may be updated with new demos, + # so we need to updaet subtask boundaries again + randomized_subtask_boundaries = ( + self.randomize_subtask_boundaries() + ) # shape [N, S, 2], last dim is start and end action lengths + prev_src_demo_datagen_info_pool_size = len(self.src_demo_datagen_info_pool.datagen_infos) + + # generate trajectory for a subtask for the eef that is currently at the beginning of a subtask + for eef_name, eef_subtask_step_index in current_eef_subtask_step_indices.items(): + if eef_subtask_step_index is None: + # current_eef_selected_src_demo_indices will be updated in generate_trajectory + subtask_trajectory = self.generate_trajectory( + env_id, + eef_name, + current_eef_subtask_indices[eef_name], + randomized_subtask_boundaries, + runtime_subtask_constraints_dict, + current_eef_selected_src_demo_indices, + current_eef_subtask_trajectories, + ) + current_eef_subtask_trajectories[eef_name] = subtask_trajectory + current_eef_subtask_step_indices[eef_name] = 0 + # current_eef_selected_src_demo_indices[eef_name] = selected_src_demo_inds + # two_arm_trajectories[task_spec_idx] = subtask_trajectory + # prev_executed_traj[task_spec_idx] = subtask_trajectory + + # determine the next waypoint for each eef based on the current subtask constraints + eef_waypoint_dict = {} + for eef_name in sorted(self.env_cfg.subtask_configs.keys()): + # handle constraints + step_ind = current_eef_subtask_step_indices[eef_name] + subtask_ind = current_eef_subtask_indices[eef_name] + if (eef_name, subtask_ind) in runtime_subtask_constraints_dict: + task_constraint = runtime_subtask_constraints_dict[(eef_name, subtask_ind)] + if task_constraint["type"] == SubTaskConstraintType._SEQUENTIAL_LATTER: + min_time_diff = task_constraint["min_time_diff"] + if not task_constraint["fulfilled"]: + if ( + min_time_diff == -1 + or step_ind >= len(current_eef_subtask_trajectories[eef_name]) - min_time_diff + ): + if step_ind > 0: + # wait at the same step + step_ind -= 1 + current_eef_subtask_step_indices[eef_name] = step_ind + + elif task_constraint["type"] == SubTaskConstraintType.COORDINATION: + synchronous_steps = task_constraint["synchronous_steps"] + concurrent_task_spec_key = task_constraint["concurrent_task_spec_key"] + concurrent_subtask_ind = task_constraint["concurrent_subtask_ind"] + concurrent_task_fulfilled = runtime_subtask_constraints_dict[ + (concurrent_task_spec_key, concurrent_subtask_ind) + ]["fulfilled"] + + if ( + task_constraint["coordination_synchronize_start"] + and current_eef_subtask_indices[concurrent_task_spec_key] < concurrent_subtask_ind + ): + # the concurrent eef is not yet at the concurrent subtask, so wait at the first action + # this also makes sure that the concurrent task starts at the same time as this task + step_ind = 0 + current_eef_subtask_step_indices[eef_name] = 0 + else: + if ( + not concurrent_task_fulfilled + and step_ind >= len(current_eef_subtask_trajectories[eef_name]) - synchronous_steps + ): + # trigger concurrent task + runtime_subtask_constraints_dict[(concurrent_task_spec_key, concurrent_subtask_ind)][ + "fulfilled" + ] = True + + if not task_constraint["fulfilled"]: + if step_ind >= len(current_eef_subtask_trajectories[eef_name]) - synchronous_steps: + if step_ind > 0: + step_ind -= 1 + current_eef_subtask_step_indices[eef_name] = step_ind # wait here + + waypoint = current_eef_subtask_trajectories[eef_name][step_ind] + eef_waypoint_dict[eef_name] = waypoint + multi_waypoint = MultiWaypoint(eef_waypoint_dict) + + # execute the next waypoints for all eefs + exec_results = await multi_waypoint.execute( + env=self.env, + success_term=success_term, + env_id=env_id, + env_action_queue=env_action_queue, + ) + + # update execution state buffers + if len(exec_results["states"]) > 0: + generated_states.extend(exec_results["states"]) + generated_obs.extend(exec_results["observations"]) + generated_actions.extend(exec_results["actions"]) + generated_success = generated_success or exec_results["success"] + + for eef_name in self.env_cfg.subtask_configs.keys(): + current_eef_subtask_step_indices[eef_name] += 1 + subtask_ind = current_eef_subtask_indices[eef_name] + if current_eef_subtask_step_indices[eef_name] == len( + current_eef_subtask_trajectories[eef_name] + ): # subtask done + if (eef_name, subtask_ind) in runtime_subtask_constraints_dict: + task_constraint = runtime_subtask_constraints_dict[(eef_name, subtask_ind)] + if task_constraint["type"] == SubTaskConstraintType._SEQUENTIAL_FORMER: + constrained_task_spec_key = task_constraint["constrained_task_spec_key"] + constrained_subtask_ind = task_constraint["constrained_subtask_ind"] + runtime_subtask_constraints_dict[(constrained_task_spec_key, constrained_subtask_ind)][ + "fulfilled" + ] = True + elif task_constraint["type"] == SubTaskConstraintType.COORDINATION: + concurrent_task_spec_key = task_constraint["concurrent_task_spec_key"] + concurrent_subtask_ind = task_constraint["concurrent_subtask_ind"] + # concurrent_task_spec_idx = task_spec_keys.index(concurrent_task_spec_key) + task_constraint["finished"] = True + # check if concurrent task has been finished + assert ( + runtime_subtask_constraints_dict[(concurrent_task_spec_key, concurrent_subtask_ind)][ + "finished" + ] + or current_eef_subtask_step_indices[concurrent_task_spec_key] + >= len(current_eef_subtask_trajectories[concurrent_task_spec_key]) - 1 + ) + + if pause_subtask: + input( + f"Pausing after subtask {current_eef_subtask_indices[eef_name]} of {eef_name} execution." + " Press any key to continue..." + ) + # This is a check to see if this arm has completed all the subtasks + if current_eef_subtask_indices[eef_name] == len(self.env_cfg.subtask_configs[eef_name]) - 1: + eef_subtasks_done[eef_name] = True + # If all subtasks done for this arm, repeat last waypoint to make sure this arm does not move + current_eef_subtask_trajectories[eef_name].append( + current_eef_subtask_trajectories[eef_name][-1] + ) + else: + current_eef_subtask_step_indices[eef_name] = None + current_eef_subtask_indices[eef_name] += 1 + # Check if all eef_subtasks_done values are True + if all(eef_subtasks_done.values()): + break + + # merge numpy arrays + if len(generated_actions) > 0: + generated_actions = torch.cat(generated_actions, dim=0) + + # set success to the recorded episode data and export to file + self.env.recorder_manager.set_success_to_episodes( + env_id_tensor, torch.tensor([[generated_success]], dtype=torch.bool, device=self.env.device) + ) + if export_demo: + self.env.recorder_manager.export_episodes(env_id_tensor) + + results = dict( + initial_state=new_initial_state, + states=generated_states, + observations=generated_obs, + actions=generated_actions, + success=generated_success, + ) + return results diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info.py new file mode 100644 index 00000000000..8a6a48681b5 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info.py @@ -0,0 +1,94 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +""" +Defines structure of information that is needed from an environment for data generation. +""" +from copy import deepcopy + + +class DatagenInfo: + """ + Defines the structure of information required from an environment for data generation processes. + The `DatagenInfo` class centralizes all essential data elements needed for data generation in one place, + reducing the overhead and complexity of repeatedly querying the environment whenever this information is needed. + + To allow for flexibility,not all information must be present. + + Core Elements: + - **eef_pose**: Captures the current 6 dimensional poses of the robot's end-effector. + - **object_poses**: Captures the 6 dimensional poses of relevant objects in the scene. + - **subtask_term_signals**: Captures subtask completions signals. + - **target_eef_pose**: Captures the target 6 dimensional poses for robot's end effector at each time step. + - **gripper_action**: Captures the gripper's state. + """ + + def __init__( + self, + eef_pose=None, + object_poses=None, + subtask_term_signals=None, + target_eef_pose=None, + gripper_action=None, + ): + """ + Args: + eef_pose (torch.Tensor or None): robot end effector poses of shape [..., 4, 4] + object_poses (dict or None): dictionary mapping object name to object poses + of shape [..., 4, 4] + subtask_term_signals (dict or None): dictionary mapping subtask name to a binary + indicator (0 or 1) on whether subtask has been completed. Each value in the + dictionary could be an int, float, or torch.Tensor of shape [..., 1]. + target_eef_pose (torch.Tensor or None): target end effector poses of shape [..., 4, 4] + gripper_action (torch.Tensor or None): gripper actions of shape [..., D] where D + is the dimension of the gripper actuation action for the robot arm + """ + self.eef_pose = None + if eef_pose is not None: + self.eef_pose = eef_pose + + self.object_poses = None + if object_poses is not None: + self.object_poses = {k: object_poses[k] for k in object_poses} + + self.subtask_term_signals = None + if subtask_term_signals is not None: + self.subtask_term_signals = dict() + for k in subtask_term_signals: + if isinstance(subtask_term_signals[k], (float, int)): + self.subtask_term_signals[k] = subtask_term_signals[k] + else: + # only create torch tensor if value is not a single value + self.subtask_term_signals[k] = subtask_term_signals[k] + + self.target_eef_pose = None + if target_eef_pose is not None: + self.target_eef_pose = target_eef_pose + + self.gripper_action = None + if gripper_action is not None: + self.gripper_action = gripper_action + + def to_dict(self): + """ + Convert this instance to a dictionary containing the same information. + """ + ret = dict() + if self.eef_pose is not None: + ret["eef_pose"] = self.eef_pose + if self.object_poses is not None: + ret["object_poses"] = deepcopy(self.object_poses) + if self.subtask_term_signals is not None: + ret["subtask_term_signals"] = deepcopy(self.subtask_term_signals) + if self.target_eef_pose is not None: + ret["target_eef_pose"] = self.target_eef_pose + if self.gripper_action is not None: + ret["gripper_action"] = self.gripper_action + return ret diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info_pool.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info_pool.py new file mode 100644 index 00000000000..599e692ae47 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/datagen_info_pool.py @@ -0,0 +1,192 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import asyncio + +from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler + +from isaaclab_mimic.datagen.datagen_info import DatagenInfo + + +class DataGenInfoPool: + """ + Pool of DatagenInfo for data generation. + + This class is a container for storing `DatagenInfo` objects that are extracted from episodes. + The pool supports the use of an asyncio lock to safely add new episodes to the pool while + consuming the data, so it can be shared across multiple mimic data generators. + """ + + def __init__(self, env, env_cfg, device, asyncio_lock: asyncio.Lock | None = None): + """ + Args: + env_cfg (dict): environment configuration + device (torch.device): device to store the data + asyncio_lock (asyncio.Lock or None): asyncio lock to use for thread safety + """ + self._datagen_infos = [] + + # Start and end step indices of each subtask in each episode for each eef + self._subtask_boundaries: dict[str, list[list[tuple[int, int]]]] = {} + + self.env = env + self.env_cfg = env_cfg + self.device = device + + self._asyncio_lock = asyncio_lock + + # Subtask termination infos for the given environment + self.subtask_term_signal_names: dict[str, list[str]] = {} + self.subtask_term_offset_ranges: dict[str, list[tuple[int, int]]] = {} + for eef_name, eef_subtask_configs in env_cfg.subtask_configs.items(): + self.subtask_term_signal_names[eef_name] = [ + subtask_config.subtask_term_signal for subtask_config in eef_subtask_configs + ] + self.subtask_term_offset_ranges[eef_name] = [ + subtask_config.subtask_term_offset_range for subtask_config in eef_subtask_configs + ] + + @property + def datagen_infos(self): + """Returns the datagen infos.""" + return self._datagen_infos + + @property + def subtask_boundaries(self) -> dict[str, list[list[tuple[int, int]]]]: + """Returns the subtask boundaries.""" + return self._subtask_boundaries + + @property + def asyncio_lock(self): + """Returns the asyncio lock.""" + return self._asyncio_lock + + @property + def num_datagen_infos(self): + """Returns the number of datagen infos.""" + return len(self._datagen_infos) + + async def add_episode(self, episode: EpisodeData): + """ + Add a datagen info from the given episode. + + Args: + episode (EpisodeData): episode to add + """ + if self._asyncio_lock is not None: + async with self._asyncio_lock: + self._add_episode(episode) + else: + self._add_episode(episode) + + def _add_episode(self, episode: EpisodeData): + """ + Add a datagen info from the given episode. + + Args: + episode (EpisodeData): episode to add + """ + ep_grp = episode.data + + # extract datagen info + if "datagen_info" in ep_grp["obs"]: + eef_pose = ep_grp["obs"]["datagen_info"]["eef_pose"] + object_poses_dict = ep_grp["obs"]["datagen_info"]["object_pose"] + target_eef_pose = ep_grp["obs"]["datagen_info"]["target_eef_pose"] + subtask_term_signals_dict = ep_grp["obs"]["datagen_info"]["subtask_term_signals"] + else: + raise ValueError("Episode to be loaded to DatagenInfo pool lacks datagen_info annotations") + + # Extract gripper actions + gripper_actions = self.env.actions_to_gripper_actions(ep_grp["actions"]) + + ep_datagen_info_obj = DatagenInfo( + eef_pose=eef_pose, + object_poses=object_poses_dict, + subtask_term_signals=subtask_term_signals_dict, + target_eef_pose=target_eef_pose, + gripper_action=gripper_actions, + ) + self._datagen_infos.append(ep_datagen_info_obj) + + # parse subtask ranges using subtask termination signals and store + # the start and end indices of each subtask for each eef + for eef_name in self.subtask_term_signal_names.keys(): + if eef_name not in self._subtask_boundaries: + self._subtask_boundaries[eef_name] = [] + prev_subtask_term_ind = 0 + eef_subtask_boundaries = [] + for subtask_term_signal_name in self.subtask_term_signal_names[eef_name]: + if subtask_term_signal_name is None: + # None refers to the final subtask, so finishes at end of demo + subtask_term_ind = ep_grp["actions"].shape[0] + else: + # trick to detect index where first 0 -> 1 transition occurs - this will be the end of the subtask + subtask_indicators = ( + ep_datagen_info_obj.subtask_term_signals[subtask_term_signal_name].flatten().int() + ) + diffs = subtask_indicators[1:] - subtask_indicators[:-1] + end_ind = int(diffs.nonzero()[0][0]) + 1 + subtask_term_ind = end_ind + 1 # increment to support indexing like demo[start:end] + + if subtask_term_ind <= prev_subtask_term_ind: + raise ValueError( + f"subtask termination signal is not increasing: {subtask_term_ind} should be greater than" + f" {prev_subtask_term_ind}" + ) + eef_subtask_boundaries.append((prev_subtask_term_ind, subtask_term_ind)) + prev_subtask_term_ind = subtask_term_ind + + # run sanity check on subtask_term_offset_range in task spec to make sure we can never + # get an empty subtask in the worst case when sampling subtask bounds: + # + # end index of subtask i + max offset of subtask i < end index of subtask i + 1 + min offset of subtask i + 1 + # + for i in range(1, len(eef_subtask_boundaries)): + prev_max_offset_range = self.subtask_term_offset_ranges[eef_name][i - 1][1] + assert ( + eef_subtask_boundaries[i - 1][1] + prev_max_offset_range + < eef_subtask_boundaries[i][1] + self.subtask_term_offset_ranges[eef_name][i][0] + ), ( + "subtask sanity check violation in demo with subtask {} end ind {}, subtask {} max offset {}," + " subtask {} end ind {}, and subtask {} min offset {}".format( + i - 1, + eef_subtask_boundaries[i - 1][1], + i - 1, + prev_max_offset_range, + i, + eef_subtask_boundaries[i][1], + i, + self.subtask_term_offset_ranges[eef_name][i][0], + ) + ) + + self._subtask_boundaries[eef_name].append(eef_subtask_boundaries) + + def load_from_dataset_file(self, file_path, select_demo_keys: str | None = None): + """ + Load from a dataset file. + + Args: + file_path (str): path to the dataset file + select_demo_keys (str or None): keys of the demos to load + """ + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.open(file_path) + episode_names = dataset_file_handler.get_episode_names() + + if len(episode_names) == 0: + return + + for episode_name in episode_names: + if select_demo_keys is not None and episode_name not in select_demo_keys: + continue + episode = dataset_file_handler.load_episode(episode_name, self.device) + self._add_episode(episode) diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/generation.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/generation.py new file mode 100644 index 00000000000..70f7c7d19ce --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/generation.py @@ -0,0 +1,237 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import asyncio +import contextlib +import torch +from typing import Any + +from isaaclab.envs import ManagerBasedRLMimicEnv +from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg +from isaaclab.managers import DatasetExportMode, TerminationTermCfg + +from isaaclab_mimic.datagen.data_generator import DataGenerator +from isaaclab_mimic.datagen.datagen_info_pool import DataGenInfoPool + +from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + +# global variable to keep track of the data generation statistics +num_success = 0 +num_failures = 0 +num_attempts = 0 + + +async def run_data_generator( + env: ManagerBasedRLMimicEnv, + env_id: int, + env_reset_queue: asyncio.Queue, + env_action_queue: asyncio.Queue, + data_generator: DataGenerator, + success_term: TerminationTermCfg, + pause_subtask: bool = False, +): + """Run mimic data generation from the given data generator in the specified environment index. + + Args: + env: The environment to run the data generator on. + env_id: The environment index to run the data generation on. + env_reset_queue: The asyncio queue to send environment (for this particular env_id) reset requests to. + env_action_queue: The asyncio queue to send actions to for executing actions. + data_generator: The data generator instance to use. + success_term: The success termination term to use. + pause_subtask: Whether to pause the subtask during generation. + """ + global num_success, num_failures, num_attempts + while True: + results = await data_generator.generate( + env_id=env_id, + success_term=success_term, + env_reset_queue=env_reset_queue, + env_action_queue=env_action_queue, + pause_subtask=pause_subtask, + ) + if bool(results["success"]): + num_success += 1 + else: + num_failures += 1 + num_attempts += 1 + + +def env_loop( + env: ManagerBasedRLMimicEnv, + env_reset_queue: asyncio.Queue, + env_action_queue: asyncio.Queue, + shared_datagen_info_pool: DataGenInfoPool, + asyncio_event_loop: asyncio.AbstractEventLoop, +): + """Main asyncio loop for the environment. + + Args: + env: The environment to run the main step loop on. + env_reset_queue: The asyncio queue to handle reset request the environment. + env_action_queue: The asyncio queue to handle actions to for executing actions. + shared_datagen_info_pool: The shared datagen info pool that stores source demo info. + asyncio_event_loop: The main asyncio event loop. + """ + global num_success, num_failures, num_attempts + env_id_tensor = torch.tensor([0], dtype=torch.int64, device=env.device) + prev_num_attempts = 0 + # simulate environment -- run everything in inference mode + with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): + while True: + + # check if any environment needs to be reset while waiting for actions + while env_action_queue.qsize() != env.num_envs: + asyncio_event_loop.run_until_complete(asyncio.sleep(0)) + while not env_reset_queue.empty(): + env_id_tensor[0] = env_reset_queue.get_nowait() + env.reset(env_ids=env_id_tensor) + env_reset_queue.task_done() + + actions = torch.zeros(env.action_space.shape) + + # get actions from all the data generators + for i in range(env.num_envs): + # an async-blocking call to get an action from a data generator + env_id, action = asyncio_event_loop.run_until_complete(env_action_queue.get()) + actions[env_id] = action + + # perform action on environment + env.step(actions) + + # mark done so the data generators can continue with the step results + for i in range(env.num_envs): + env_action_queue.task_done() + + if prev_num_attempts != num_attempts: + prev_num_attempts = num_attempts + generated_sucess_rate = 100 * num_success / num_attempts if num_attempts > 0 else 0.0 + print("") + print("*" * 50, "\033[K") + print( + f"{num_success}/{num_attempts} ({generated_sucess_rate:.1f}%) successful demos generated by" + " mimic\033[K" + ) + print("*" * 50, "\033[K") + + # termination condition is on enough successes if @guarantee_success or enough attempts otherwise + generation_guarantee = env.cfg.datagen_config.generation_guarantee + generation_num_trials = env.cfg.datagen_config.generation_num_trials + check_val = num_success if generation_guarantee else num_attempts + if check_val >= generation_num_trials: + print(f"Reached {generation_num_trials} successes/attempts. Exiting.") + break + + # check that simulation is stopped or not + if env.sim.is_stopped(): + break + + env.close() + + +def setup_env_config( + env_name: str, + output_dir: str, + output_file_name: str, + num_envs: int, + device: str, + generation_num_trials: int | None = None, +) -> tuple[Any, Any]: + """Configure the environment for data generation. + + Args: + env_name: Name of the environment + output_dir: Directory to save output + output_file_name: Name of output file + num_envs: Number of environments to run + device: Device to run on + generation_num_trials: Optional override for number of trials + + Returns: + tuple containing: + - env_cfg: The environment configuration + - success_term: The success termination condition + + Raises: + NotImplementedError: If no success termination term found + """ + env_cfg = parse_env_cfg(env_name, device=device, num_envs=num_envs) + + if generation_num_trials is not None: + env_cfg.datagen_config.generation_num_trials = generation_num_trials + + env_cfg.env_name = env_name + + # Extract success checking function + success_term = None + if hasattr(env_cfg.terminations, "success"): + success_term = env_cfg.terminations.success + env_cfg.terminations.success = None + else: + raise NotImplementedError("No success termination term was found in the environment.") + + # Configure for data generation + env_cfg.terminations = None + env_cfg.observations.policy.concatenate_terms = False + + # Setup recorders + env_cfg.recorders = ActionStateRecorderManagerCfg() + env_cfg.recorders.dataset_export_dir_path = output_dir + env_cfg.recorders.dataset_filename = output_file_name + + if env_cfg.datagen_config.generation_keep_failed: + env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_SUCCEEDED_FAILED_IN_SEPARATE_FILES + else: + env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_SUCCEEDED_ONLY + + return env_cfg, success_term + + +def setup_async_generation( + env: Any, num_envs: int, input_file: str, success_term: Any, pause_subtask: bool = False +) -> dict[str, Any]: + """Setup async data generation tasks. + + Args: + env: The environment instance + num_envs: Number of environments to run + input_file: Path to input dataset file + success_term: Success termination condition + pause_subtask: Whether to pause after subtasks + + Returns: + List of asyncio tasks for data generation + """ + asyncio_event_loop = asyncio.get_event_loop() + env_reset_queue = asyncio.Queue() + env_action_queue = asyncio.Queue() + shared_datagen_info_pool_lock = asyncio.Lock() + shared_datagen_info_pool = DataGenInfoPool(env, env.cfg, env.device, asyncio_lock=shared_datagen_info_pool_lock) + shared_datagen_info_pool.load_from_dataset_file(input_file) + print(f"Loaded {shared_datagen_info_pool.num_datagen_infos} to datagen info pool") + + # Create and schedule data generator tasks + data_generator = DataGenerator(env=env, src_demo_datagen_info_pool=shared_datagen_info_pool) + data_generator_asyncio_tasks = [] + for i in range(num_envs): + task = asyncio_event_loop.create_task( + run_data_generator( + env, i, env_reset_queue, env_action_queue, data_generator, success_term, pause_subtask=pause_subtask + ) + ) + data_generator_asyncio_tasks.append(task) + + return { + "tasks": data_generator_asyncio_tasks, + "event_loop": asyncio_event_loop, + "reset_queue": env_reset_queue, + "action_queue": env_action_queue, + "info_pool": shared_datagen_info_pool, + } diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/selection_strategy.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/selection_strategy.py new file mode 100644 index 00000000000..b433dd5c2e6 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/selection_strategy.py @@ -0,0 +1,313 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +""" +Selection strategies used by Isaac Lab Mimic to select subtask segments from +source human demonstrations. +""" +import abc # for abstract base class definitions +import torch + +import isaaclab.utils.math as PoseUtils + +# Global dictionary for remembering name to class mappings. +REGISTERED_SELECTION_STRATEGIES = {} + + +def make_selection_strategy(name, *args, **kwargs): + """ + Creates an instance of a selection strategy class, specified by @name, + which is used to look it up in the registry. + """ + assert_selection_strategy_exists(name) + return REGISTERED_SELECTION_STRATEGIES[name](*args, **kwargs) + + +def register_selection_strategy(cls): + """ + Register selection strategy class into global registry. + """ + ignore_classes = ["SelectionStrategy"] + if cls.__name__ not in ignore_classes: + REGISTERED_SELECTION_STRATEGIES[cls.NAME] = cls + + +def assert_selection_strategy_exists(name): + """ + Allow easy way to check if selection strategy exists. + """ + if name not in REGISTERED_SELECTION_STRATEGIES: + raise Exception( + "assert_selection_strategy_exists: name {} not found. Make sure it is a registered selection strategy" + " among {}".format(name, ", ".join(REGISTERED_SELECTION_STRATEGIES)) + ) + + +class SelectionStrategyMeta(type): + """ + This metaclass adds selection strategy classes into the global registry. + """ + + def __new__(meta, name, bases, class_dict): + cls = super().__new__(meta, name, bases, class_dict) + register_selection_strategy(cls) + return cls + + +class SelectionStrategy(metaclass=SelectionStrategyMeta): + """ + Defines methods and functions for selection strategies to implement. + """ + + def __init__(self): + pass + + @property + @classmethod + def NAME(self): + """ + This name (str) will be used to register the selection strategy class in the global + registry. + """ + raise NotImplementedError + + @abc.abstractmethod + def select_source_demo( + self, + eef_pose, + object_pose, + src_subtask_datagen_infos, + ): + """ + Selects source demonstration index using the current robot pose, relevant object pose + for the current subtask, and relevant information from the source demonstrations for the + current subtask. + + Args: + eef_pose (torch.Tensor): current 4x4 eef pose + object_pose (torch.Tensor): current 4x4 object pose, for the object in this subtask + src_subtask_datagen_infos (list): DatagenInfo instance for the relevant subtask segment + in the source demonstrations + + Returns: + source_demo_ind (int): index of source demonstration - indicates which source subtask segment to use + """ + raise NotImplementedError + + +class RandomStrategy(SelectionStrategy): + """ + Pick source demonstration randomly. + """ + + # name for registering this class into registry + NAME = "random" + + def select_source_demo( + self, + eef_pose, + object_pose, + src_subtask_datagen_infos, + ): + """ + Selects source demonstration index using the current robot pose, relevant object pose + for the current subtask, and relevant information from the source demonstrations for the + current subtask. + + Args: + eef_pose (torch.Tensor): current 4x4 eef pose + object_pose (torch.Tensor): current 4x4 object pose, for the object in this subtask + src_subtask_datagen_infos (list): DatagenInfo instance for the relevant subtask segment + in the source demonstrations + + Returns: + source_demo_ind (int): index of source demonstration - indicates which source subtask segment to use + """ + + # random selection + n_src_demo = len(src_subtask_datagen_infos) + return torch.randint(0, n_src_demo, (1,)).item() + + +class NearestNeighborObjectStrategy(SelectionStrategy): + """ + Pick source demonstration to be the one with the closest object pose to the object + in the current scene. + """ + + # name for registering this class into registry + NAME = "nearest_neighbor_object" + + def select_source_demo( + self, + eef_pose, + object_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=3, + ): + """ + Selects source demonstration index using the current robot pose, relevant object pose + for the current subtask, and relevant information from the source demonstrations for the + current subtask. + + Args: + eef_pose (torch.Tensor): current 4x4 eef pose + object_pose (torch.Tensor): current 4x4 object pose, for the object in this subtask + src_subtask_datagen_infos (list): DatagenInfo instance for the relevant subtask segment + in the source demonstrations + pos_weight (float): weight on position for minimizing pose distance + rot_weight (float): weight on rotation for minimizing pose distance + nn_k (int): pick source demo index uniformly at randomly from the top @nn_k nearest neighbors + + Returns: + source_demo_ind (int): index of source demonstration - indicates which source subtask segment to use + """ + + # collect object poses from start of subtask source segments into tensor of shape [N, 4, 4] + src_object_poses = [] + for di in src_subtask_datagen_infos: + src_obj_pose = list(di.object_poses.values()) + assert len(src_obj_pose) == 1 + # use object pose at start of subtask segment + src_object_poses.append(src_obj_pose[0][0]) + src_object_poses = torch.stack(src_object_poses) + + # split into positions and rotations + all_src_obj_pos, all_src_obj_rot = PoseUtils.unmake_pose(src_object_poses) + obj_pos, obj_rot = PoseUtils.unmake_pose(object_pose) + + # prepare for broadcasting + obj_pos = obj_pos.view(-1, 3) + obj_rot_T = obj_rot.transpose(0, 1).view(-1, 3, 3) + + # pos dist is just L2 between positions + pos_dists = torch.sqrt(((all_src_obj_pos - obj_pos) ** 2).sum(dim=-1)) + + # get angle (in axis-angle representation of delta rotation matrix) using the following formula + # (see http://www.boris-belousov.net/2016/12/01/quat-dist/) + + # batched matrix mult, [N, 3, 3] x [1, 3, 3] -> [N, 3, 3] + delta_R = torch.matmul(all_src_obj_rot, obj_rot_T) + arc_cos_in = (torch.diagonal(delta_R, dim1=-2, dim2=-1).sum(dim=-1) - 1.0) / 2.0 + arc_cos_in = torch.clamp(arc_cos_in, -1.0, 1.0) # clip for numerical stability + rot_dists = torch.acos(arc_cos_in) + + # weight distances with coefficients + dists_to_minimize = pos_weight * pos_dists + rot_weight * rot_dists + + # clip top-k parameter to max possible value + nn_k = min(nn_k, len(dists_to_minimize)) + + # return one of the top-K nearest neighbors uniformly at random + rand_k = torch.randint(0, nn_k, (1,)).item() + top_k_neighbors_in_order = torch.argsort(dists_to_minimize)[:nn_k] + return top_k_neighbors_in_order[rand_k] + + +class NearestNeighborRobotDistanceStrategy(SelectionStrategy): + """ + Pick source demonstration to be the one that minimizes the distance the robot + end effector will need to travel from the current pose to the first pose + in the transformed segment. + """ + + # name for registering this class into registry + NAME = "nearest_neighbor_robot_distance" + + def select_source_demo( + self, + eef_pose, + object_pose, + src_subtask_datagen_infos, + pos_weight=1.0, + rot_weight=1.0, + nn_k=3, + ): + """ + Selects source demonstration index using the current robot pose, relevant object pose + for the current subtask, and relevant information from the source demonstrations for the + current subtask. + + Args: + eef_pose (torch.Tensor): current 4x4 eef pose + object_pose (torch.Tensor): current 4x4 object pose, for the object in this subtask + src_subtask_datagen_infos (list): DatagenInfo instance for the relevant subtask segment + in the source demonstrations + pos_weight (float): weight on position for minimizing pose distance + rot_weight (float): weight on rotation for minimizing pose distance + nn_k (int): pick source demo index uniformly at randomly from the top @nn_k nearest neighbors + + Returns: + source_demo_ind (int): index of source demonstration - indicates which source subtask segment to use + """ + + # collect eef and object poses from start of subtask source segments into tensors of shape [N, 4, 4] + src_eef_poses = [] + src_object_poses = [] + for di in src_subtask_datagen_infos: + # use eef pose at start of subtask segment + src_eef_poses.append(di.eef_pose[0]) + # use object pose at start of subtask segment + src_obj_pose = list(di.object_poses.values()) + assert len(src_obj_pose) == 1 + src_object_poses.append(src_obj_pose[0][0]) + src_eef_poses = torch.stack(src_eef_poses) + src_object_poses = torch.stack(src_object_poses) + + # Get source eef poses with respect to object frames. + # note: frame A is world, frame B is object + src_object_poses_inv = PoseUtils.pose_inv(src_object_poses) + src_eef_poses_in_obj = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_poses, + pose_A_in_B=src_object_poses_inv, + ) + + # Use this to find the first pose for the transformed subtask segment for each source demo. + # Note this is the same logic used in PoseUtils.transform_poses_from_frame_A_to_frame_B + transformed_eef_poses = PoseUtils.pose_in_A_to_pose_in_B( + pose_in_A=src_eef_poses_in_obj, + pose_A_in_B=object_pose, + ) + + # split into positions and rotations + all_transformed_eef_pos, all_transformed_eef_rot = PoseUtils.unmake_pose(transformed_eef_poses) + eef_pos, eef_rot = PoseUtils.unmake_pose(eef_pose) + + # now measure distance from each of these transformed eef poses to our current eef pose + # and choose the source demo that minimizes this distance + + # prepare for broadcasting + eef_pos = eef_pos.view(-1, 3) + eef_rot_T = eef_rot.transpose(0, 1).view(-1, 3, 3) + + # pos dist is just L2 between positions + pos_dists = torch.sqrt(((all_transformed_eef_pos - eef_pos) ** 2).sum(dim=-1)) + + # get angle (in axis-angle representation of delta rotation matrix) using the following formula + # (see http://www.boris-belousov.net/2016/12/01/quat-dist/) + + # batched matrix mult, [N, 3, 3] x [1, 3, 3] -> [N, 3, 3] + delta_R = torch.matmul(all_transformed_eef_rot, eef_rot_T) + arc_cos_in = (torch.diagonal(delta_R, dim1=-2, dim2=-1).sum(dim=-1) - 1.0) / 2.0 + arc_cos_in = torch.clamp(arc_cos_in, -1.0, 1.0) # clip for numerical stability + rot_dists = torch.acos(arc_cos_in) + + # weight distances with coefficients + dists_to_minimize = pos_weight * pos_dists + rot_weight * rot_dists + + # clip top-k parameter to max possible value + nn_k = min(nn_k, len(dists_to_minimize)) + + # return one of the top-K nearest neighbors uniformly at random + rand_k = torch.randint(0, nn_k, (1,)).item() + top_k_neighbors_in_order = torch.argsort(dists_to_minimize)[:nn_k] + return top_k_neighbors_in_order[rand_k] diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/utils.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/utils.py new file mode 100644 index 00000000000..ee2e95f3c9c --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/utils.py @@ -0,0 +1,218 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from collections.abc import Callable +from typing import Any + +from IPython.display import display +from ipywidgets import widgets + +from isaaclab.envs import ManagerBasedEnv +from isaaclab.managers import EventTermCfg +from isaaclab.utils.datasets import HDF5DatasetFileHandler + + +def get_nested_value(d: dict[str, Any], keys: list[str]) -> Any: + """Retrieve a nested value from dictionary d using list of keys.""" + for k in keys: + d = d[k] + return d + + +def update_nested_value(d: dict[str, Any], keys: list[str], value: Any) -> None: + """Update a nested value in dictionary d using list of keys.""" + for k in keys[:-1]: + d = d.setdefault(k, {}) + d[keys[-1]] = value + + +def reset_env(env: ManagerBasedEnv, steps: int = 1) -> None: + """Reset environment and step simulation to stabilize state.""" + # Get sim and scene from unwrapped environment + sim = env.unwrapped.sim + scene = env.unwrapped.scene + + # Reset environment + env.reset() + + # Step simulation multiple times to stabilize + for _ in range(steps): + # Write data to sim + scene.write_data_to_sim() + # Perform step + sim.step() + # Update buffers + scene.update(dt=env.physics_dt) + + +def get_parameter_input( + param_name: str, + current_val: float | tuple[float, float] | list[float], + allowed_range: tuple[float, float, float | None], + update_fn: Callable[[float | tuple[float, float]], None], + env: ManagerBasedEnv | None = None, + event_term_name: str | None = None, +) -> widgets.FloatSlider | widgets.FloatRangeSlider: + """Get parameter input using ipywidgets with immediate value updates.""" + + if isinstance(current_val, (tuple, list)): + step_size = allowed_range[2] if len(allowed_range) > 2 else 0.01 + full_param_name = f"{event_term_name}.{param_name}" if event_term_name else param_name + + # Create container with label and range slider + container = widgets.HBox([ + widgets.Label(full_param_name, layout=widgets.Layout(width="auto")), + widgets.FloatRangeSlider( + value=[current_val[0], current_val[1]], + min=allowed_range[0], + max=allowed_range[1], + step=step_size, + layout=widgets.Layout(width="300px"), + readout=True, + readout_format=".3f", + ), + ]) + + def on_value_change(change): + new_tuple = (change["new"][0], change["new"][1]) + update_fn(new_tuple) + if env is not None: + reset_env(env, steps=50) + + container.children[1].observe(on_value_change, names="value") + + # Create help text showing the allowed range + help_text = widgets.HTML(value=f'

Allowed range: {allowed_range[:2]}

') + + display(container) + display(help_text) + + return container.children[1] + else: + step_size = allowed_range[2] if len(allowed_range) > 2 else 0.01 + full_param_name = f"{event_term_name}.{param_name}" if event_term_name else param_name + + # Create container with label and slider + container = widgets.HBox([ + widgets.Label(full_param_name, layout=widgets.Layout(width="auto")), + widgets.FloatSlider( + value=current_val, + min=allowed_range[0], + max=allowed_range[1], + step=step_size, + layout=widgets.Layout(width="300px"), + readout=True, + readout_format=".3f", + ), + ]) + + def on_value_change(change): + update_fn(change["new"]) + if env is not None: + reset_env(env, steps=50) + + container.children[1].observe(on_value_change, names="value") + + # Create help text showing the allowed range + help_text = widgets.HTML(value=f'

Allowed range: {allowed_range[:2]}

') + + display(container) + display(help_text) + + return container.children[1] + + +def interactive_update_randomizable_params( + event_term: EventTermCfg, + event_term_name: str, + param_config: dict[str, dict | tuple[float, float, float | None]], + param_path: str = "", + env: ManagerBasedEnv | None = None, +) -> list[tuple[list[str], widgets.FloatSlider | widgets.FloatRangeSlider]]: + """Interactive parameter updates using ipywidgets.""" + inputs = [] + + for key, allowed_range in param_config.items(): + current_path = f"{param_path}.{key}" if param_path else key + keys = current_path.split(".") + + if isinstance(allowed_range, dict): + interactive_update_randomizable_params(event_term, event_term_name, allowed_range, current_path, env) + else: + try: + current_val = get_nested_value(event_term.params, keys) + + def make_update_fn(k, full_path): + def update_fn(new_val): + update_nested_value(event_term.params, k, new_val) + print(f"Updated '{full_path}' to {new_val}.") + + return update_fn + + input_widget = get_parameter_input( + current_path, + current_val, + allowed_range, + make_update_fn(keys, current_path), + env=env, + event_term_name=event_term_name, + ) + inputs.append((keys, input_widget)) + except KeyError: + print(f"Key '{current_path}' not found in event_term.params; skipping.") + continue + + return inputs + + +def setup_output_paths(output_file_path: str) -> tuple[str, str]: + """Set up output directory and get file name for dataset generation. + + Args: + output_file_path: Full path to the desired output file + + Returns: + tuple containing: + - output_dir: Path to the output directory + - output_file_name: Name of the output file without extension + """ + output_dir = os.path.dirname(output_file_path) + output_file_name = os.path.splitext(os.path.basename(output_file_path))[0] + + # create directory if it does not exist + if not os.path.exists(output_dir): + os.makedirs(output_dir) + + return output_dir, output_file_name + + +def get_env_name_from_dataset(input_file_path: str) -> str: + """Get environment name from an input dataset file. + + Args: + input_file_path: Path to the input dataset file + + Returns: + env_name: Name of the environment from the dataset + + Raises: + FileNotFoundError: If the input file does not exist + """ + if not os.path.exists(input_file_path): + raise FileNotFoundError(f"The dataset file {input_file_path} does not exist.") + + dataset_file_handler = HDF5DatasetFileHandler() + dataset_file_handler.open(input_file_path) + env_name = dataset_file_handler.get_env_name() + if env_name is None: + raise ValueError("Environment name not found in dataset") + + return env_name diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/waypoint.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/waypoint.py new file mode 100644 index 00000000000..163b419593c --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/datagen/waypoint.py @@ -0,0 +1,431 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +""" +A collection of classes used to represent waypoints and trajectories. +""" +import asyncio +import inspect +import torch +from copy import deepcopy + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv +from isaaclab.managers import TerminationTermCfg + + +class Waypoint: + """ + Represents a single desired 6-DoF waypoint, along with corresponding gripper actuation for this point. + """ + + def __init__(self, pose, gripper_action, noise=None): + """ + Args: + pose (torch.Tensor): 4x4 pose target for robot controller + gripper_action (torch.Tensor): gripper action for robot controller + noise (float or None): action noise amplitude to apply during execution at this timestep + (for arm actions, not gripper actions) + """ + self.pose = pose + self.gripper_action = gripper_action + self.noise = noise + + def __str__(self): + """String representation of the waypoint.""" + return f"Waypoint:\n Pose:\n{self.pose}\n" + + +class WaypointSequence: + """ + Represents a sequence of Waypoint objects. + """ + + def __init__(self, sequence=None): + """ + Args: + sequence (list or None): if provided, should be a list of Waypoint objects + """ + if sequence is None: + self.sequence = [] + else: + for waypoint in sequence: + assert isinstance(waypoint, Waypoint) + self.sequence = deepcopy(sequence) + + @classmethod + def from_poses(cls, poses, gripper_actions, action_noise): + """ + Instantiate a WaypointSequence object given a sequence of poses, + gripper actions, and action noise. + + Args: + poses (torch.Tensor): sequence of pose matrices of shape (T, 4, 4) + gripper_actions (torch.Tensor): sequence of gripper actions + that should be applied at each timestep of shape (T, D). + action_noise (float or torch.Tensor): sequence of action noise + magnitudes that should be applied at each timestep. If a + single float is provided, the noise magnitude will be + constant over the trajectory. + """ + assert isinstance(action_noise, (float, torch.Tensor)) + + # handle scalar to tensor conversion + num_timesteps = poses.shape[0] + if isinstance(action_noise, float): + action_noise = action_noise * torch.ones((num_timesteps, 1), dtype=torch.float32) + action_noise = action_noise.reshape(-1, 1) + + # make WaypointSequence instance + sequence = [ + Waypoint( + pose=poses[t], + gripper_action=gripper_actions[t], + noise=action_noise[t, 0], + ) + for t in range(num_timesteps) + ] + return cls(sequence=sequence) + + def get_poses(self): + poses = [] + for waypoint in self.sequence: + poses.append(waypoint.pose[:2, 3]) + return poses + + def __len__(self): + # length of sequence + return len(self.sequence) + + def __getitem__(self, ind): + """ + Returns waypoint at index. + + Returns: + waypoint (Waypoint instance) + """ + return self.sequence[ind] + + def __add__(self, other): + """ + Defines addition (concatenation) of sequences + """ + return WaypointSequence(sequence=(self.sequence + other.sequence)) + + def __str__(self): + """Prints all waypoints in the sequence.""" + output = [] + for idx, waypoint in enumerate(self.sequence): + output.append(f"Waypoint {idx}: {waypoint}") + return "\n".join(output) + + @property + def last_waypoint(self): + """ + Return last waypoint in sequence. + + Returns: + waypoint (Waypoint instance) + """ + return deepcopy(self.sequence[-1]) + + def split(self, ind): + """ + Splits this sequence into 2 pieces, the part up to time index @ind, and the + rest. Returns 2 WaypointSequence objects. + """ + seq_1 = self.sequence[:ind] + seq_2 = self.sequence[ind:] + return WaypointSequence(sequence=seq_1), WaypointSequence(sequence=seq_2) + + +class WaypointTrajectory: + """ + A sequence of WaypointSequence objects that corresponds to a full 6-DoF trajectory. + """ + + def __init__(self): + self.waypoint_sequences = [] + + def __len__(self): + # sum up length of all waypoint sequences + return sum(len(s) for s in self.waypoint_sequences) + + def __getitem__(self, ind): + """ + Returns waypoint at time index. + + Returns: + waypoint (Waypoint instance) + """ + assert len(self.waypoint_sequences) > 0 + assert (ind >= 0) and (ind < len(self)) + + # find correct waypoint sequence we should index + end_ind = 0 + for seq_ind in range(len(self.waypoint_sequences)): + start_ind = end_ind + end_ind += len(self.waypoint_sequences[seq_ind]) + if (ind >= start_ind) and (ind < end_ind): + break + + # index within waypoint sequence + return self.waypoint_sequences[seq_ind][ind - start_ind] + + @property + def last_waypoint(self): + """ + Return last waypoint in sequence. + + Returns: + waypoint (Waypoint instance) + """ + return self.waypoint_sequences[-1].last_waypoint + + def get_poses(self): + poses = [] + for waypoint_sequence in self.waypoint_sequences: + for waypoint in waypoint_sequence: + poses.append(waypoint.pose[:2, 3]) + return poses + + def add_waypoint_sequence(self, sequence): + """ + Directly append sequence to list (no interpolation). + + Args: + sequence (WaypointSequence instance): sequence to add + """ + assert isinstance(sequence, WaypointSequence) + self.waypoint_sequences.append(sequence) + + def add_waypoint_sequence_for_target_pose( + self, + pose, + gripper_action, + num_steps, + skip_interpolation=False, + action_noise=0.0, + ): + """ + Adds a new waypoint sequence corresponding to a desired target pose. A new WaypointSequence + will be constructed consisting of @num_steps intermediate Waypoint objects. These can either + be constructed with linear interpolation from the last waypoint (default) or be a + constant set of target poses (set @skip_interpolation to True). + + Args: + pose (torch.Tensor): 4x4 target pose + + gripper_action (torch.Tensor): value for gripper action + + num_steps (int): number of action steps when trying to reach this waypoint. Will + add intermediate linearly interpolated points between the last pose on this trajectory + and the target pose, so that the total number of steps is @num_steps. + + skip_interpolation (bool): if True, keep the target pose fixed and repeat it @num_steps + times instead of using linearly interpolated targets. + + action_noise (float): scale of random gaussian noise to add during action execution (e.g. + when @execute is called) + """ + if len(self.waypoint_sequences) == 0: + assert skip_interpolation, "cannot interpolate since this is the first waypoint sequence" + + if skip_interpolation: + # repeat the target @num_steps times + assert num_steps is not None + poses = pose.unsqueeze(0).repeat((num_steps, 1, 1)) + gripper_actions = gripper_action.unsqueeze(0).repeat((num_steps, 1)) + else: + # linearly interpolate between the last pose and the new waypoint + last_waypoint = self.last_waypoint + poses, num_steps_2 = PoseUtils.interpolate_poses( + pose_1=last_waypoint.pose, + pose_2=pose, + num_steps=num_steps, + ) + assert num_steps == num_steps_2 + gripper_actions = gripper_action.unsqueeze(0).repeat((num_steps + 2, 1)) + # make sure to skip the first element of the new path, which already exists on the current trajectory path + poses = poses[1:] + gripper_actions = gripper_actions[1:] + + # add waypoint sequence for this set of poses + sequence = WaypointSequence.from_poses( + poses=poses, + gripper_actions=gripper_actions, + action_noise=action_noise, + ) + self.add_waypoint_sequence(sequence) + + def pop_first(self): + """ + Removes first waypoint in first waypoint sequence and returns it. If the first waypoint + sequence is now empty, it is also removed. + + Returns: + waypoint (Waypoint instance) + """ + first, rest = self.waypoint_sequences[0].split(1) + if len(rest) == 0: + # remove empty waypoint sequence + self.waypoint_sequences = self.waypoint_sequences[1:] + else: + # update first waypoint sequence + self.waypoint_sequences[0] = rest + return first + + def merge( + self, + other, + num_steps_interp=None, + num_steps_fixed=None, + action_noise=0.0, + ): + """ + Merge this trajectory with another (@other). + + Args: + other (WaypointTrajectory object): the other trajectory to merge into this one + + num_steps_interp (int or None): if not None, add a waypoint sequence that interpolates + between the end of the current trajectory and the start of @other + + num_steps_fixed (int or None): if not None, add a waypoint sequence that has constant + target poses corresponding to the first target pose in @other + + action_noise (float): noise to use during the interpolation segment + """ + need_interp = (num_steps_interp is not None) and (num_steps_interp > 0) + need_fixed = (num_steps_fixed is not None) and (num_steps_fixed > 0) + use_interpolation_segment = need_interp or need_fixed + + if use_interpolation_segment: + # pop first element of other trajectory + other_first = other.pop_first() + + # Get first target pose of other trajectory. + # The interpolated segment will include this first element as its last point. + target_for_interpolation = other_first[0] + + if need_interp: + # interpolation segment + self.add_waypoint_sequence_for_target_pose( + pose=target_for_interpolation.pose, + gripper_action=target_for_interpolation.gripper_action, + num_steps=num_steps_interp, + action_noise=action_noise, + skip_interpolation=False, + ) + + if need_fixed: + # segment of constant target poses equal to @other's first target pose + + # account for the fact that we pop'd the first element of @other in anticipation of an interpolation segment + num_steps_fixed_to_use = num_steps_fixed if need_interp else (num_steps_fixed + 1) + self.add_waypoint_sequence_for_target_pose( + pose=target_for_interpolation.pose, + gripper_action=target_for_interpolation.gripper_action, + num_steps=num_steps_fixed_to_use, + action_noise=action_noise, + skip_interpolation=True, + ) + + # make sure to preserve noise from first element of other trajectory + self.waypoint_sequences[-1][-1].noise = target_for_interpolation.noise + + # concatenate the trajectories + self.waypoint_sequences += other.waypoint_sequences + + def get_full_sequence(self): + """ + Returns the full sequence of waypoints in the trajectory. + + Returns: + sequence (WaypointSequence instance) + """ + return WaypointSequence(sequence=[waypoint for seq in self.waypoint_sequences for waypoint in seq.sequence]) + + +class MultiWaypoint: + """ + A collection of Waypoint objects for multiple end effectors in the environment. + """ + + def __init__(self, waypoints: dict[str, Waypoint]): + """ + Args: + waypoints (dict): a dictionary of waypionts of end effectors + """ + self.waypoints = waypoints + + async def execute( + self, + env: ManagerBasedRLMimicEnv, + success_term: TerminationTermCfg, + env_id: int = 0, + env_action_queue: asyncio.Queue | None = None, + ): + """ + Executes the multi-waypoint eef actions in the environment. + + Args: + env: The environment to execute the multi-waypoint actions in. + success_term: The termination term to check for task success. + env_id: The environment ID to execute the multi-waypoint actions in. + env_action_queue: The asyncio queue to put the action into. + + Returns: + A dictionary containing the state, observation, action, and success of the multi-waypoint actions. + """ + # current state + state = env.scene.get_state(is_relative=True) + + # construct action from target poses and gripper actions + target_eef_pose_dict = {eef_name: waypoint.pose for eef_name, waypoint in self.waypoints.items()} + gripper_action_dict = {eef_name: waypoint.gripper_action for eef_name, waypoint in self.waypoints.items()} + if "action_noise_dict" in inspect.signature(env.target_eef_pose_to_action).parameters: + action_noise_dict = {eef_name: waypoint.noise for eef_name, waypoint in self.waypoints.items()} + play_action = env.target_eef_pose_to_action( + target_eef_pose_dict=target_eef_pose_dict, + gripper_action_dict=gripper_action_dict, + action_noise_dict=action_noise_dict, + env_id=env_id, + ) + else: + # calling user-defined env.target_eef_pose_to_action() with noise parameter is deprecated + # (replaced by action_noise_dict) + play_action = env.target_eef_pose_to_action( + target_eef_pose_dict=target_eef_pose_dict, + gripper_action_dict=gripper_action_dict, + noise=max([waypoint.noise for waypoint in self.waypoints.values()]), + env_id=env_id, + ) + + if play_action.dim() == 1: + play_action = play_action.unsqueeze(0) # Reshape with additional env dimension + + # step environment + if env_action_queue is None: + obs, _, _, _, _ = env.step(play_action) + else: + await env_action_queue.put((env_id, play_action[0])) + await env_action_queue.join() + obs = env.obs_buf + + success = bool(success_term.func(env, **success_term.params)[env_id]) + + result = dict( + states=[state], + observations=[obs], + actions=[play_action], + success=success, + ) + return result diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/__init__.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/__init__.py new file mode 100644 index 00000000000..e21ad079495 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/__init__.py @@ -0,0 +1,60 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with environment wrappers for Isaac Lab Mimic.""" + +import gymnasium as gym + +from .franka_stack_ik_abs_mimic_env import FrankaCubeStackIKAbsMimicEnv +from .franka_stack_ik_abs_mimic_env_cfg import FrankaCubeStackIKAbsMimicEnvCfg +from .franka_stack_ik_rel_blueprint_mimic_env_cfg import FrankaCubeStackIKRelBlueprintMimicEnvCfg +from .franka_stack_ik_rel_mimic_env import FrankaCubeStackIKRelMimicEnv +from .franka_stack_ik_rel_mimic_env_cfg import FrankaCubeStackIKRelMimicEnvCfg +from .franka_stack_ik_rel_visuomotor_mimic_env_cfg import FrankaCubeStackIKRelVisuomotorMimicEnvCfg + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Mimic-v0", + entry_point="isaaclab_mimic.envs:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": franka_stack_ik_rel_mimic_env_cfg.FrankaCubeStackIKRelMimicEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-Mimic-v0", + entry_point="isaaclab_mimic.envs:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": franka_stack_ik_rel_blueprint_mimic_env_cfg.FrankaCubeStackIKRelBlueprintMimicEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs:FrankaCubeStackIKAbsMimicEnv", + kwargs={ + "env_cfg_entry_point": franka_stack_ik_abs_mimic_env_cfg.FrankaCubeStackIKAbsMimicEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Mimic-v0", + entry_point="isaaclab_mimic.envs:FrankaCubeStackIKRelMimicEnv", + kwargs={ + "env_cfg_entry_point": franka_stack_ik_rel_visuomotor_mimic_env_cfg.FrankaCubeStackIKRelVisuomotorMimicEnvCfg, + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py new file mode 100644 index 00000000000..48048c93b7f --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env.py @@ -0,0 +1,103 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class FrankaCubeStackIKAbsMimicEnv(ManagerBasedRLMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for Franka Cube Stack IK Abs env. + """ + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """Get current robot end effector pose.""" + if env_ids is None: + env_ids = slice(None) + + # Retrieve end effector pose from the observation buffer + eef_pos = self.obs_buf["policy"]["eef_pos"][env_ids] + eef_quat = self.obs_buf["policy"]["eef_quat"][env_ids] + # Quaternion format is w,x,y,z + return PoseUtils.make_pose(eef_pos, PoseUtils.matrix_from_quat(eef_quat)) + + def target_eef_pose_to_action( + self, target_eef_pose_dict: dict, gripper_action_dict: dict, noise: float | None = None, env_id: int = 0 + ) -> torch.Tensor: + """Convert target pose to action. + + This method transforms a dictionary of target end-effector poses and gripper actions + into a single action tensor that can be used by the environment. + + The function: + 1. Extracts target position and rotation from the pose dictionary + 2. Extracts gripper action for the end effector + 3. Concatenates position and quaternion rotation into a pose action + 4. Optionally adds noise to the pose action for exploration + 5. Combines pose action with gripper action into a final action tensor + + Args: + target_eef_pose_dict: Dictionary containing target end-effector pose(s), + with keys as eef names and values as pose tensors. + gripper_action_dict: Dictionary containing gripper action(s), + with keys as eef names and values as action tensors. + noise: Optional noise magnitude to apply to the pose action for exploration. + If provided, random noise is generated and added to the pose action. + env_id: Environment ID for multi-environment setups, defaults to 0. + + Returns: + torch.Tensor: A single action tensor combining pose and gripper commands. + """ + # target position and rotation + (target_eef_pose,) = target_eef_pose_dict.values() + target_pos, target_rot = PoseUtils.unmake_pose(target_eef_pose) + + # get gripper action for single eef + (gripper_action,) = gripper_action_dict.values() + + # add noise to action + pose_action = torch.cat([target_pos, PoseUtils.quat_from_matrix(target_rot)], dim=0) + if noise is not None: + noise = noise * torch.randn_like(pose_action) + pose_action += noise + + return torch.cat([pose_action, gripper_action], dim=0).unsqueeze(0) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """Convert action to target pose.""" + eef_name = list(self.cfg.subtask_configs.keys())[0] + + target_pos = action[:, :3] + target_quat = action[:, 3:7] + target_rot = PoseUtils.matrix_from_quat(target_quat) + + target_poses = PoseUtils.make_pose(target_pos, target_rot).clone() + + return {eef_name: target_poses} + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """Extract gripper actions.""" + # last dimension is gripper action + return {list(self.cfg.subtask_configs.keys())[0]: actions[:, -1:]} + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """Get subtask termination signals.""" + if env_ids is None: + env_ids = slice(None) + + signals = dict() + subtask_terms = self.obs_buf["subtask_terms"] + signals["grasp_1"] = subtask_terms["grasp_1"][env_ids] + signals["grasp_2"] = subtask_terms["grasp_2"][env_ids] + signals["stack_1"] = subtask_terms["stack_1"][env_ids] + return signals diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py new file mode 100644 index 00000000000..88e5976eb9a --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_abs_mimic_env_cfg.py @@ -0,0 +1,92 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_ik_abs_env_cfg import FrankaCubeStackEnvCfg + + +@configclass +class FrankaCubeStackIKAbsMimicEnvCfg(FrankaCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Franka Cube Stack IK Abs env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "demo_src_stack_isaac_lab_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + object_ref="cube_2", + subtask_term_signal="grasp_1", + subtask_term_offset_range=(10, 20), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.01, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_1", + subtask_term_signal="stack_1", + subtask_term_offset_range=(10, 20), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.01, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_3", + subtask_term_signal="grasp_2", + subtask_term_offset_range=(10, 20), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.01, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + object_ref="cube_2", + subtask_term_signal=None, + subtask_term_offset_range=(0, 0), + selection_strategy="nearest_neighbor_object", + selection_strategy_kwargs={"nn_k": 3}, + action_noise=0.01, + num_interpolation_steps=5, + num_fixed_steps=0, + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["franka"] = subtask_configs diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py new file mode 100644 index 00000000000..556ca407e78 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_blueprint_mimic_env_cfg.py @@ -0,0 +1,132 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_ik_rel_blueprint_env_cfg import ( + FrankaCubeStackBlueprintEnvCfg, +) + + +@configclass +class FrankaCubeStackIKRelBlueprintMimicEnvCfg(FrankaCubeStackBlueprintEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Franka Cube Stack IK Rel env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "isaac_lab_franka_stack_ik_rel_blueprint_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="grasp_1", + # Specifies time offsets for data generation when splitting a trajectory into + # subtask segments. Random offsets are added to the termination boundary. + subtask_term_offset_range=(10, 20), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_1", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="stack_1", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_3", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="grasp_2", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["franka"] = subtask_configs diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py new file mode 100644 index 00000000000..f96466ed24f --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env.py @@ -0,0 +1,170 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class FrankaCubeStackIKRelMimicEnv(ManagerBasedRLMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for Franka Cube Stack IK Rel env. + """ + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + # Retrieve end effector pose from the observation buffer + eef_pos = self.obs_buf["policy"]["eef_pos"][env_ids] + eef_quat = self.obs_buf["policy"]["eef_quat"][env_ids] + # Quaternion format is w,x,y,z + return PoseUtils.make_pose(eef_pos, PoseUtils.matrix_from_quat(eef_quat)) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + noise: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + eef_name = list(self.cfg.subtask_configs.keys())[0] + + # target position and rotation + (target_eef_pose,) = target_eef_pose_dict.values() + target_pos, target_rot = PoseUtils.unmake_pose(target_eef_pose) + + # current position and rotation + curr_pose = self.get_robot_eef_pose(eef_name, env_ids=[env_id])[0] + curr_pos, curr_rot = PoseUtils.unmake_pose(curr_pose) + + # normalized delta position action + delta_position = target_pos - curr_pos + + # normalized delta rotation action + delta_rot_mat = target_rot.matmul(curr_rot.transpose(-1, -2)) + delta_quat = PoseUtils.quat_from_matrix(delta_rot_mat) + delta_rotation = PoseUtils.axis_angle_from_quat(delta_quat) + + # get gripper action for single eef + (gripper_action,) = gripper_action_dict.values() + + # add noise to action + pose_action = torch.cat([delta_position, delta_rotation], dim=0) + if action_noise_dict is not None: + noise = action_noise_dict["franka"] * torch.randn_like(pose_action) + pose_action += noise + pose_action = torch.clamp(pose_action, -1.0, 1.0) + + return torch.cat([pose_action, gripper_action], dim=0) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim) + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to + """ + eef_name = list(self.cfg.subtask_configs.keys())[0] + + delta_position = action[:, :3] + delta_rotation = action[:, 3:6] + + # current position and rotation + curr_pose = self.get_robot_eef_pose(eef_name, env_ids=None) + curr_pos, curr_rot = PoseUtils.unmake_pose(curr_pose) + + # get pose target + target_pos = curr_pos + delta_position + + # Convert delta_rotation to axis angle form + delta_rotation_angle = torch.linalg.norm(delta_rotation, dim=-1, keepdim=True) + delta_rotation_axis = delta_rotation / delta_rotation_angle + + # Handle invalid division for the case when delta_rotation_angle is close to zero + is_close_to_zero_angle = torch.isclose(delta_rotation_angle, torch.zeros_like(delta_rotation_angle)).squeeze(1) + delta_rotation_axis[is_close_to_zero_angle] = torch.zeros_like(delta_rotation_axis)[is_close_to_zero_angle] + + delta_quat = PoseUtils.quat_from_angle_axis(delta_rotation_angle.squeeze(1), delta_rotation_axis).squeeze(0) + delta_rot_mat = PoseUtils.matrix_from_quat(delta_quat) + target_rot = torch.matmul(delta_rot_mat, curr_rot) + + target_poses = PoseUtils.make_pose(target_pos, target_rot).clone() + + return {eef_name: target_poses} + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + # last dimension is gripper action + return {list(self.cfg.subtask_configs.keys())[0]: actions[:, -1:]} + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask term signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask term signal annotation. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + if env_ids is None: + env_ids = slice(None) + + signals = dict() + subtask_terms = self.obs_buf["subtask_terms"] + signals["grasp_1"] = subtask_terms["grasp_1"][env_ids] + signals["grasp_2"] = subtask_terms["grasp_2"][env_ids] + signals["stack_1"] = subtask_terms["stack_1"][env_ids] + # final subtask is placing cubeC on cubeA (motion relative to cubeA) - but final subtask signal is not needed + return signals diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py new file mode 100644 index 00000000000..9221fe6ddbc --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_mimic_env_cfg.py @@ -0,0 +1,138 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_ik_rel_env_cfg import FrankaCubeStackEnvCfg + + +@configclass +class FrankaCubeStackIKRelMimicEnvCfg(FrankaCubeStackEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Franka Cube Stack IK Rel env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + # # TODO: Figure out how we can move this to the MimicEnvCfg class + # # The __post_init__() above only calls the init for FrankaCubeStackEnvCfg and not MimicEnvCfg + # # https://stackoverflow.com/questions/59986413/achieving-multiple-inheritance-using-python-dataclasses + + # Override the existing values + self.datagen_config.name = "demo_src_stack_isaac_lab_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.generation_relative = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="grasp_1", + # Specifies time offsets for data generation when splitting a trajectory into + # subtask segments. Random offsets are added to the termination boundary. + subtask_term_offset_range=(10, 20), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + description="Grasp red cube", + next_subtask_description="Stack red cube on top of blue cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_1", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="stack_1", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + next_subtask_description="Grasp green cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_3", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="grasp_2", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + next_subtask_description="Stack green cube on top of red cube", + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["franka"] = subtask_configs diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py new file mode 100644 index 00000000000..4134ce7c418 --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/franka_stack_ik_rel_visuomotor_mimic_env_cfg.py @@ -0,0 +1,133 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_ik_rel_visuomotor_env_cfg import ( + FrankaCubeStackVisuomotorEnvCfg, +) + + +@configclass +class FrankaCubeStackIKRelVisuomotorMimicEnvCfg(FrankaCubeStackVisuomotorEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Franka Cube Stack IK Rel Visuomotor env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "isaac_lab_franka_stack_ik_rel_visuomotor_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.generation_relative = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="grasp_1", + # Specifies time offsets for data generation when splitting a trajectory into + # subtask segments. Random offsets are added to the termination boundary. + subtask_term_offset_range=(10, 20), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_1", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="stack_1", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_3", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal="grasp_2", + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(10, 20), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="cube_2", + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.03, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["franka"] = subtask_configs diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/__init__.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/__init__.py new file mode 100644 index 00000000000..519f5630dac --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -0,0 +1,25 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with environment wrappers for Isaac Lab Mimic.""" + +import gymnasium as gym + +from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv +from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg + +gym.register( + id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs.pinocchio_envs:PickPlaceGR1T2MimicEnv", + kwargs={ + "env_cfg_entry_point": pickplace_gr1t2_mimic_env_cfg.PickPlaceGR1T2MimicEnvCfg, + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py new file mode 100644 index 00000000000..0a6c912d4cc --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py @@ -0,0 +1,135 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class PickPlaceGR1T2MimicEnv(ManagerBasedRLMimicEnv): + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + eef_pos_name = f"{eef_name}_eef_pos" + eef_quat_name = f"{eef_name}_eef_quat" + + target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_wrist_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + noise: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + + # target position and rotation + target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"]) + target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"]) + + target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) + target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) + + # gripper actions + left_gripper_action = gripper_action_dict["left"] + right_gripper_action = gripper_action_dict["right"] + + if action_noise_dict is not None: + pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) + pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) + quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) + quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) + + target_left_eef_pos += pos_noise_left + target_right_eef_pos += pos_noise_right + target_left_eef_rot_quat += quat_noise_left + target_right_eef_rot_quat += quat_noise_right + + return torch.cat( + ( + target_left_eef_pos, + target_left_eef_rot_quat, + target_right_eef_pos, + target_right_eef_rot_quat, + left_gripper_action, + right_gripper_action, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + target_poses = {} + + target_left_wrist_position = action[:, 0:3] + target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) + target_poses["left"] = target_pose_left + + target_right_wrist_position = action[:, 7:10] + target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) + target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) + target_poses["right"] = target_pose_right + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + return {"left": actions[:, 14:25], "right": actions[:, 25:]} diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py new file mode 100644 index 00000000000..2dd4df01d2b --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py @@ -0,0 +1,114 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_env_cfg import PickPlaceGR1T2EnvCfg + + +@configclass +class PickPlaceGR1T2MimicEnvCfg(PickPlaceGR1T2EnvCfg, MimicEnvCfg): + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "demo_src_gr1t2_demo_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = False + self.datagen_config.generation_num_trials = 1000 + self.datagen_config.generation_select_src_per_subtask = False + self.datagen_config.generation_select_src_per_arm = False + self.datagen_config.generation_relative = False + self.datagen_config.generation_joint_pos = False + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.num_demo_to_render = 10 + self.datagen_config.num_fail_demo_to_render = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="idle_right", + first_subtask_start_offset_range=(0, 0), + # Randomization range for starting index of the first subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for the source subtask segment during data generation + # selection_strategy="nearest_neighbor_object", + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=3, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["right"] = subtask_configs + + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.005, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["left"] = subtask_configs diff --git a/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/ui/instruction_display.py b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/ui/instruction_display.py new file mode 100644 index 00000000000..da4d6f643ea --- /dev/null +++ b/install/isaaclab_mimic/lib/python3.10/site-packages/isaaclab_mimic/ui/instruction_display.py @@ -0,0 +1,104 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Module for handling instruction displays in Isaac Lab environments.""" + +from typing import Any + +from pxr import Gf + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg + + +class InstructionDisplay: + """Handles instruction display for different teleop devices.""" + + def __init__(self, teleop_device): + self.teleop_device = teleop_device.lower() + + if self.teleop_device == "handtracking": + from isaaclab.ui.xr_widgets import show_instruction + + self._display_subtask = lambda text: show_instruction( + text, "/_xr/stage/xrCamera", Gf.Vec3f(1.25, 0.3, -2), target_prim_path="/subtask_instruction" + ) + self._display_demo = lambda text: show_instruction( + text, "/_xr/stage/xrCamera", Gf.Vec3f(-1.25, 0.3, -2), target_prim_path="/demo_complete" + ) + else: + self.subtask_label = None + self.demo_label = None + self._display_subtask = lambda text: setattr(self.subtask_label, "text", text) + self._display_demo = lambda text: setattr(self.demo_label, "text", text) + + def set_labels(self, subtask_label, demo_label): + """Set the instruction labels for non-handtracking displays.""" + self.subtask_label = subtask_label + self.demo_label = demo_label + + def show_subtask(self, text): + """Display subtask instruction.""" + self._display_subtask(text) + + def show_demo(self, text): + """Display demo completion message.""" + self._display_demo(text) + + +def show_subtask_instructions( + instruction_display: InstructionDisplay, prev_subtasks: dict, obv: dict, env_cfg: Any +) -> None: + """ + Detect changes in subtasks and display the changes. + + Args: + instruction_display: Display handler for showing instructions + prev_subtasks: Previous subtask terms + obv: Current observation with subtask terms + env_cfg: Environment configuration containing subtask descriptions + """ + if not isinstance(env_cfg, MimicEnvCfg): + return + subtasks = obv[0].get("subtask_terms") + if subtasks is None: + return + + # Currently only supports one end effector + eef_name = list(env_cfg.subtask_configs.keys())[0] + subtask_configs = env_cfg.subtask_configs[eef_name] + + all_false = True + for subtask_config in subtask_configs: + term_signal = subtask_config.subtask_term_signal + if term_signal is None: + continue + + current_state = subtasks[term_signal].item() + prev_state = prev_subtasks.get(term_signal, False) + + if current_state: + all_false = False + + # Show message when state changes from False to True + if current_state and not prev_state: + instruction_display.show_subtask(f"Next objective: {subtask_config.next_subtask_description}") + + # Update the previous state + prev_subtasks[term_signal] = current_state + + # If all tasks are false, show the first task's description + if all_false and subtask_configs: + first_task = subtask_configs[0] + instruction_display.show_subtask(f"Current objective: {first_task.description}") diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/__init__.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/__init__.py new file mode 100644 index 00000000000..f7103c9a126 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/__init__.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package for environment wrappers to different learning frameworks. + +Wrappers allow you to modify the behavior of an environment without modifying the environment itself. +This is useful for modifying the observation space, action space, or reward function. Additionally, +they can be used to cast a given environment into the respective environment class definition used by +different learning frameworks. This operation may include handling of asymmetric actor-critic observations, +casting the data between different backends such `numpy` and `pytorch`, or organizing the returned data +into the expected data structure by the learning framework. + +All wrappers work similar to the :class:`gymnasium.Wrapper` class. Using a wrapper is as simple as passing +the initialized environment instance to the wrapper constructor. However, since learning frameworks +expect different input and output data structures, their wrapper classes are not compatible with each other. +Thus, they should always be used in conjunction with the respective learning framework. +""" diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rl_games.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rl_games.py new file mode 100644 index 00000000000..7067563596d --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rl_games.py @@ -0,0 +1,355 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an environment instance to RL-Games vectorized environment. + +The following example shows how to wrap an environment for RL-Games and register the environment construction +for RL-Games :class:`Runner` class: + +.. code-block:: python + + from rl_games.common import env_configurations, vecenv + + from isaaclab_rl.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper + + # configuration parameters + rl_device = "cuda:0" + clip_obs = 10.0 + clip_actions = 1.0 + + # wrap around environment for rl-games + env = RlGamesVecEnvWrapper(env, rl_device, clip_obs, clip_actions) + + # register the environment to rl-games registry + # note: in agents configuration: environment name must be "rlgpu" + vecenv.register( + "IsaacRlgWrapper", lambda config_name, num_actors, **kwargs: RlGamesGpuEnv(config_name, num_actors, **kwargs) + ) + env_configurations.register("rlgpu", {"vecenv_type": "IsaacRlgWrapper", "env_creator": lambda **kwargs: env}) + +""" + +# needed to import for allowing type-hinting:gym.spaces.Box | None +from __future__ import annotations + +import gym.spaces # needed for rl-games incompatibility: https://github.com/Denys88/rl_games/issues/261 +import gymnasium +import torch + +from rl_games.common import env_configurations +from rl_games.common.vecenv import IVecEnv + +from isaaclab.envs import DirectRLEnv, ManagerBasedRLEnv, VecEnvObs + +""" +Vectorized environment wrapper. +""" + + +class RlGamesVecEnvWrapper(IVecEnv): + """Wraps around Isaac Lab environment for RL-Games. + + This class wraps around the Isaac Lab environment. Since RL-Games works directly on + GPU buffers, the wrapper handles moving of buffers from the simulation environment + to the same device as the learning agent. Additionally, it performs clipping of + observations and actions. + + For algorithms like asymmetric actor-critic, RL-Games expects a dictionary for + observations. This dictionary contains "obs" and "states" which typically correspond + to the actor and critic observations respectively. + + To use asymmetric actor-critic, the environment observations from :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv` + must have the key or group name "critic". The observation group is used to set the + :attr:`num_states` (int) and :attr:`state_space` (:obj:`gym.spaces.Box`). These are + used by the learning agent in RL-Games to allocate buffers in the trajectory memory. + Since this is optional for some environments, the wrapper checks if these attributes exist. + If they don't then the wrapper defaults to zero as number of privileged observations. + + .. caution:: + + This class must be the last wrapper in the wrapper chain. This is because the wrapper does not follow + the :class:`gym.Wrapper` interface. Any subsequent wrappers will need to be modified to work with this + wrapper. + + + Reference: + https://github.com/Denys88/rl_games/blob/master/rl_games/common/ivecenv.py + https://github.com/NVIDIA-Omniverse/IsaacGymEnvs + """ + + def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, rl_device: str, clip_obs: float, clip_actions: float): + """Initializes the wrapper instance. + + Args: + env: The environment to wrap around. + rl_device: The device on which agent computations are performed. + clip_obs: The clipping value for observations. + clip_actions: The clipping value for actions. + + Raises: + ValueError: The environment is not inherited from :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. + ValueError: If specified, the privileged observations (critic) are not of type :obj:`gym.spaces.Box`. + """ + # check that input is valid + if not isinstance(env.unwrapped, ManagerBasedRLEnv) and not isinstance(env.unwrapped, DirectRLEnv): + raise ValueError( + "The environment must be inherited from ManagerBasedRLEnv or DirectRLEnv. Environment type:" + f" {type(env)}" + ) + # initialize the wrapper + self.env = env + # store provided arguments + self._rl_device = rl_device + self._clip_obs = clip_obs + self._clip_actions = clip_actions + self._sim_device = env.unwrapped.device + # information for privileged observations + if self.state_space is None: + self.rlg_num_states = 0 + else: + self.rlg_num_states = self.state_space.shape[0] + + def __str__(self): + """Returns the wrapper name and the :attr:`env` representation string.""" + return ( + f"<{type(self).__name__}{self.env}>" + f"\n\tObservations clipping: {self._clip_obs}" + f"\n\tActions clipping : {self._clip_actions}" + f"\n\tAgent device : {self._rl_device}" + f"\n\tAsymmetric-learning : {self.rlg_num_states != 0}" + ) + + def __repr__(self): + """Returns the string representation of the wrapper.""" + return str(self) + + """ + Properties -- Gym.Wrapper + """ + + @property + def render_mode(self) -> str | None: + """Returns the :attr:`Env` :attr:`render_mode`.""" + return self.env.render_mode + + @property + def observation_space(self) -> gym.spaces.Box: + """Returns the :attr:`Env` :attr:`observation_space`.""" + # note: rl-games only wants single observation space + policy_obs_space = self.unwrapped.single_observation_space["policy"] + if not isinstance(policy_obs_space, gymnasium.spaces.Box): + raise NotImplementedError( + f"The RL-Games wrapper does not currently support observation space: '{type(policy_obs_space)}'." + f" If you need to support this, please modify the wrapper: {self.__class__.__name__}," + " and if you are nice, please send a merge-request." + ) + # note: maybe should check if we are a sub-set of the actual space. don't do it right now since + # in ManagerBasedRLEnv we are setting action space as (-inf, inf). + return gym.spaces.Box(-self._clip_obs, self._clip_obs, policy_obs_space.shape) + + @property + def action_space(self) -> gym.Space: + """Returns the :attr:`Env` :attr:`action_space`.""" + # note: rl-games only wants single action space + action_space = self.unwrapped.single_action_space + if not isinstance(action_space, gymnasium.spaces.Box): + raise NotImplementedError( + f"The RL-Games wrapper does not currently support action space: '{type(action_space)}'." + f" If you need to support this, please modify the wrapper: {self.__class__.__name__}," + " and if you are nice, please send a merge-request." + ) + # return casted space in gym.spaces.Box (OpenAI Gym) + # note: maybe should check if we are a sub-set of the actual space. don't do it right now since + # in ManagerBasedRLEnv we are setting action space as (-inf, inf). + return gym.spaces.Box(-self._clip_actions, self._clip_actions, action_space.shape) + + @classmethod + def class_name(cls) -> str: + """Returns the class name of the wrapper.""" + return cls.__name__ + + @property + def unwrapped(self) -> ManagerBasedRLEnv | DirectRLEnv: + """Returns the base environment of the wrapper. + + This will be the bare :class:`gymnasium.Env` environment, underneath all layers of wrappers. + """ + return self.env.unwrapped + + """ + Properties + """ + + @property + def num_envs(self) -> int: + """Returns the number of sub-environment instances.""" + return self.unwrapped.num_envs + + @property + def device(self) -> str: + """Returns the base environment simulation device.""" + return self.unwrapped.device + + @property + def state_space(self) -> gym.spaces.Box | None: + """Returns the :attr:`Env` :attr:`observation_space`.""" + # note: rl-games only wants single observation space + critic_obs_space = self.unwrapped.single_observation_space.get("critic") + # check if we even have a critic obs + if critic_obs_space is None: + return None + elif not isinstance(critic_obs_space, gymnasium.spaces.Box): + raise NotImplementedError( + f"The RL-Games wrapper does not currently support state space: '{type(critic_obs_space)}'." + f" If you need to support this, please modify the wrapper: {self.__class__.__name__}," + " and if you are nice, please send a merge-request." + ) + # return casted space in gym.spaces.Box (OpenAI Gym) + # note: maybe should check if we are a sub-set of the actual space. don't do it right now since + # in ManagerBasedRLEnv we are setting action space as (-inf, inf). + return gym.spaces.Box(-self._clip_obs, self._clip_obs, critic_obs_space.shape) + + def get_number_of_agents(self) -> int: + """Returns number of actors in the environment.""" + return getattr(self, "num_agents", 1) + + def get_env_info(self) -> dict: + """Returns the Gym spaces for the environment.""" + return { + "observation_space": self.observation_space, + "action_space": self.action_space, + "state_space": self.state_space, + } + + """ + Operations - MDP + """ + + def seed(self, seed: int = -1) -> int: # noqa: D102 + return self.unwrapped.seed(seed) + + def reset(self): # noqa: D102 + obs_dict, _ = self.env.reset() + # process observations and states + return self._process_obs(obs_dict) + + def step(self, actions): # noqa: D102 + # move actions to sim-device + actions = actions.detach().clone().to(device=self._sim_device) + # clip the actions + actions = torch.clamp(actions, -self._clip_actions, self._clip_actions) + # perform environment step + obs_dict, rew, terminated, truncated, extras = self.env.step(actions) + + # move time out information to the extras dict + # this is only needed for infinite horizon tasks + # note: only useful when `value_bootstrap` is True in the agent configuration + if not self.unwrapped.cfg.is_finite_horizon: + extras["time_outs"] = truncated.to(device=self._rl_device) + # process observations and states + obs_and_states = self._process_obs(obs_dict) + # move buffers to rl-device + # note: we perform clone to prevent issues when rl-device and sim-device are the same. + rew = rew.to(device=self._rl_device) + dones = (terminated | truncated).to(device=self._rl_device) + extras = { + k: v.to(device=self._rl_device, non_blocking=True) if hasattr(v, "to") else v for k, v in extras.items() + } + # remap extras from "log" to "episode" + if "log" in extras: + extras["episode"] = extras.pop("log") + + return obs_and_states, rew, dones, extras + + def close(self): # noqa: D102 + return self.env.close() + + """ + Helper functions + """ + + def _process_obs(self, obs_dict: VecEnvObs) -> torch.Tensor | dict[str, torch.Tensor]: + """Processing of the observations and states from the environment. + + Note: + States typically refers to privileged observations for the critic function. It is typically used in + asymmetric actor-critic algorithms. + + Args: + obs_dict: The current observations from environment. + + Returns: + If environment provides states, then a dictionary containing the observations and states is returned. + Otherwise just the observations tensor is returned. + """ + # process policy obs + obs = obs_dict["policy"] + # clip the observations + obs = torch.clamp(obs, -self._clip_obs, self._clip_obs) + # move the buffer to rl-device + obs = obs.to(device=self._rl_device).clone() + + # check if asymmetric actor-critic or not + if self.rlg_num_states > 0: + # acquire states from the environment if it exists + try: + states = obs_dict["critic"] + except AttributeError: + raise NotImplementedError("Environment does not define key 'critic' for privileged observations.") + # clip the states + states = torch.clamp(states, -self._clip_obs, self._clip_obs) + # move buffers to rl-device + states = states.to(self._rl_device).clone() + # convert to dictionary + return {"obs": obs, "states": states} + else: + return obs + + +""" +Environment Handler. +""" + + +class RlGamesGpuEnv(IVecEnv): + """Thin wrapper to create instance of the environment to fit RL-Games runner.""" + + # TODO: Adding this for now but do we really need this? + + def __init__(self, config_name: str, num_actors: int, **kwargs): + """Initialize the environment. + + Args: + config_name: The name of the environment configuration. + num_actors: The number of actors in the environment. This is not used in this wrapper. + """ + self.env: RlGamesVecEnvWrapper = env_configurations.configurations[config_name]["env_creator"](**kwargs) + + def step(self, action): # noqa: D102 + return self.env.step(action) + + def reset(self): # noqa: D102 + return self.env.reset() + + def get_number_of_agents(self) -> int: + """Get number of agents in the environment. + + Returns: + The number of agents in the environment. + """ + return self.env.get_number_of_agents() + + def get_env_info(self) -> dict: + """Get the Gym spaces for the environment. + + Returns: + The Gym spaces for the environment. + """ + return self.env.get_env_info() diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/__init__.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/__init__.py new file mode 100644 index 00000000000..82d3a16412b --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/__init__.py @@ -0,0 +1,28 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrappers and utilities to configure an environment for RSL-RL library. + +The following example shows how to wrap an environment for RSL-RL: + +.. code-block:: python + + from isaaclab_rl.rsl_rl import RslRlVecEnvWrapper + + env = RslRlVecEnvWrapper(env) + +""" + +from .distillation_cfg import * +from .exporter import export_policy_as_jit, export_policy_as_onnx +from .rl_cfg import * +from .rnd_cfg import RslRlRndCfg +from .symmetry_cfg import RslRlSymmetryCfg +from .vecenv_wrapper import RslRlVecEnvWrapper diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/distillation_cfg.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/distillation_cfg.py new file mode 100644 index 00000000000..eaeea965f00 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/distillation_cfg.py @@ -0,0 +1,88 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +######################### +# Policy configurations # +######################### + + +@configclass +class RslRlDistillationStudentTeacherCfg: + """Configuration for the distillation student-teacher networks.""" + + class_name: str = "StudentTeacher" + """The policy class name. Default is StudentTeacher.""" + + init_noise_std: float = MISSING + """The initial noise standard deviation for the student policy.""" + + noise_std_type: Literal["scalar", "log"] = "scalar" + """The type of noise standard deviation for the policy. Default is scalar.""" + + student_hidden_dims: list[int] = MISSING + """The hidden dimensions of the student network.""" + + teacher_hidden_dims: list[int] = MISSING + """The hidden dimensions of the teacher network.""" + + activation: str = MISSING + """The activation function for the student and teacher networks.""" + + +@configclass +class RslRlDistillationStudentTeacherRecurrentCfg(RslRlDistillationStudentTeacherCfg): + """Configuration for the distillation student-teacher recurrent networks.""" + + class_name: str = "StudentTeacherRecurrent" + """The policy class name. Default is StudentTeacherRecurrent.""" + + rnn_type: str = MISSING + """The type of the RNN network. Either "lstm" or "gru".""" + + rnn_hidden_dim: int = MISSING + """The hidden dimension of the RNN network.""" + + rnn_num_layers: int = MISSING + """The number of layers of the RNN network.""" + + teacher_recurrent: bool = MISSING + """Whether the teacher network is recurrent too.""" + + +############################ +# Algorithm configurations # +############################ + + +@configclass +class RslRlDistillationAlgorithmCfg: + """Configuration for the distillation algorithm.""" + + class_name: str = "Distillation" + """The algorithm class name. Default is Distillation.""" + + num_learning_epochs: int = MISSING + """The number of updates performed with each sample.""" + + learning_rate: float = MISSING + """The learning rate for the student policy.""" + + gradient_length: int = MISSING + """The number of environment steps the gradient flows back.""" + + max_grad_norm: None | float = None + """The maximum norm the gradient is clipped to.""" diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/exporter.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/exporter.py new file mode 100644 index 00000000000..3e06560c3fd --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/exporter.py @@ -0,0 +1,176 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import copy +import os +import torch + + +def export_policy_as_jit(policy: object, normalizer: object | None, path: str, filename="policy.pt"): + """Export policy into a Torch JIT file. + + Args: + policy: The policy torch module. + normalizer: The empirical normalizer module. If None, Identity is used. + path: The path to the saving directory. + filename: The name of exported JIT file. Defaults to "policy.pt". + """ + policy_exporter = _TorchPolicyExporter(policy, normalizer) + policy_exporter.export(path, filename) + + +def export_policy_as_onnx( + policy: object, path: str, normalizer: object | None = None, filename="policy.onnx", verbose=False +): + """Export policy into a Torch ONNX file. + + Args: + policy: The policy torch module. + normalizer: The empirical normalizer module. If None, Identity is used. + path: The path to the saving directory. + filename: The name of exported ONNX file. Defaults to "policy.onnx". + verbose: Whether to print the model summary. Defaults to False. + """ + if not os.path.exists(path): + os.makedirs(path, exist_ok=True) + policy_exporter = _OnnxPolicyExporter(policy, normalizer, verbose) + policy_exporter.export(path, filename) + + +""" +Helper Classes - Private. +""" + + +class _TorchPolicyExporter(torch.nn.Module): + """Exporter of actor-critic into JIT file.""" + + def __init__(self, policy, normalizer=None): + super().__init__() + self.is_recurrent = policy.is_recurrent + # copy policy parameters + if hasattr(policy, "actor"): + self.actor = copy.deepcopy(policy.actor) + if self.is_recurrent: + self.rnn = copy.deepcopy(policy.memory_a.rnn) + elif hasattr(policy, "student"): + self.actor = copy.deepcopy(policy.student) + if self.is_recurrent: + self.rnn = copy.deepcopy(policy.memory_s.rnn) + else: + raise ValueError("Policy does not have an actor/student module.") + # set up recurrent network + if self.is_recurrent: + self.rnn.cpu() + self.register_buffer("hidden_state", torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size)) + self.register_buffer("cell_state", torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size)) + self.forward = self.forward_lstm + self.reset = self.reset_memory + # copy normalizer if exists + if normalizer: + self.normalizer = copy.deepcopy(normalizer) + else: + self.normalizer = torch.nn.Identity() + + def forward_lstm(self, x): + x = self.normalizer(x) + x, (h, c) = self.rnn(x.unsqueeze(0), (self.hidden_state, self.cell_state)) + self.hidden_state[:] = h + self.cell_state[:] = c + x = x.squeeze(0) + return self.actor(x) + + def forward(self, x): + return self.actor(self.normalizer(x)) + + @torch.jit.export + def reset(self): + pass + + def reset_memory(self): + self.hidden_state[:] = 0.0 + self.cell_state[:] = 0.0 + + def export(self, path, filename): + os.makedirs(path, exist_ok=True) + path = os.path.join(path, filename) + self.to("cpu") + traced_script_module = torch.jit.script(self) + traced_script_module.save(path) + + +class _OnnxPolicyExporter(torch.nn.Module): + """Exporter of actor-critic into ONNX file.""" + + def __init__(self, policy, normalizer=None, verbose=False): + super().__init__() + self.verbose = verbose + self.is_recurrent = policy.is_recurrent + # copy policy parameters + if hasattr(policy, "actor"): + self.actor = copy.deepcopy(policy.actor) + if self.is_recurrent: + self.rnn = copy.deepcopy(policy.memory_a.rnn) + elif hasattr(policy, "student"): + self.actor = copy.deepcopy(policy.student) + if self.is_recurrent: + self.rnn = copy.deepcopy(policy.memory_s.rnn) + else: + raise ValueError("Policy does not have an actor/student module.") + # set up recurrent network + if self.is_recurrent: + self.rnn.cpu() + self.forward = self.forward_lstm + # copy normalizer if exists + if normalizer: + self.normalizer = copy.deepcopy(normalizer) + else: + self.normalizer = torch.nn.Identity() + + def forward_lstm(self, x_in, h_in, c_in): + x_in = self.normalizer(x_in) + x, (h, c) = self.rnn(x_in.unsqueeze(0), (h_in, c_in)) + x = x.squeeze(0) + return self.actor(x), h, c + + def forward(self, x): + return self.actor(self.normalizer(x)) + + def export(self, path, filename): + self.to("cpu") + if self.is_recurrent: + obs = torch.zeros(1, self.rnn.input_size) + h_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) + c_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) + actions, h_out, c_out = self(obs, h_in, c_in) + torch.onnx.export( + self, + (obs, h_in, c_in), + os.path.join(path, filename), + export_params=True, + opset_version=11, + verbose=self.verbose, + input_names=["obs", "h_in", "c_in"], + output_names=["actions", "h_out", "c_out"], + dynamic_axes={}, + ) + else: + obs = torch.zeros(1, self.actor[0].in_features) + torch.onnx.export( + self, + obs, + os.path.join(path, filename), + export_params=True, + opset_version=11, + verbose=self.verbose, + input_names=["obs"], + output_names=["actions"], + dynamic_axes={}, + ) diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rl_cfg.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rl_cfg.py new file mode 100644 index 00000000000..715ec3d4332 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rl_cfg.py @@ -0,0 +1,204 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .distillation_cfg import RslRlDistillationAlgorithmCfg, RslRlDistillationStudentTeacherCfg +from .rnd_cfg import RslRlRndCfg +from .symmetry_cfg import RslRlSymmetryCfg + +######################### +# Policy configurations # +######################### + + +@configclass +class RslRlPpoActorCriticCfg: + """Configuration for the PPO actor-critic networks.""" + + class_name: str = "ActorCritic" + """The policy class name. Default is ActorCritic.""" + + init_noise_std: float = MISSING + """The initial noise standard deviation for the policy.""" + + noise_std_type: Literal["scalar", "log"] = "scalar" + """The type of noise standard deviation for the policy. Default is scalar.""" + + actor_hidden_dims: list[int] = MISSING + """The hidden dimensions of the actor network.""" + + critic_hidden_dims: list[int] = MISSING + """The hidden dimensions of the critic network.""" + + activation: str = MISSING + """The activation function for the actor and critic networks.""" + + +@configclass +class RslRlPpoActorCriticRecurrentCfg(RslRlPpoActorCriticCfg): + """Configuration for the PPO actor-critic networks with recurrent layers.""" + + class_name: str = "ActorCriticRecurrent" + """The policy class name. Default is ActorCriticRecurrent.""" + + rnn_type: str = MISSING + """The type of RNN to use. Either "lstm" or "gru".""" + + rnn_hidden_dim: int = MISSING + """The dimension of the RNN layers.""" + + rnn_num_layers: int = MISSING + """The number of RNN layers.""" + + +############################ +# Algorithm configurations # +############################ + + +@configclass +class RslRlPpoAlgorithmCfg: + """Configuration for the PPO algorithm.""" + + class_name: str = "PPO" + """The algorithm class name. Default is PPO.""" + + num_learning_epochs: int = MISSING + """The number of learning epochs per update.""" + + num_mini_batches: int = MISSING + """The number of mini-batches per update.""" + + learning_rate: float = MISSING + """The learning rate for the policy.""" + + schedule: str = MISSING + """The learning rate schedule.""" + + gamma: float = MISSING + """The discount factor.""" + + lam: float = MISSING + """The lambda parameter for Generalized Advantage Estimation (GAE).""" + + entropy_coef: float = MISSING + """The coefficient for the entropy loss.""" + + desired_kl: float = MISSING + """The desired KL divergence.""" + + max_grad_norm: float = MISSING + """The maximum gradient norm.""" + + value_loss_coef: float = MISSING + """The coefficient for the value loss.""" + + use_clipped_value_loss: bool = MISSING + """Whether to use clipped value loss.""" + + clip_param: float = MISSING + """The clipping parameter for the policy.""" + + normalize_advantage_per_mini_batch: bool = False + """Whether to normalize the advantage per mini-batch. Default is False. + + If True, the advantage is normalized over the mini-batches only. + Otherwise, the advantage is normalized over the entire collected trajectories. + """ + + symmetry_cfg: RslRlSymmetryCfg | None = None + """The symmetry configuration. Default is None, in which case symmetry is not used.""" + + rnd_cfg: RslRlRndCfg | None = None + """The configuration for the Random Network Distillation (RND) module. Default is None, + in which case RND is not used. + """ + + +######################### +# Runner configurations # +######################### + + +@configclass +class RslRlOnPolicyRunnerCfg: + """Configuration of the runner for on-policy algorithms.""" + + seed: int = 42 + """The seed for the experiment. Default is 42.""" + + device: str = "cuda:0" + """The device for the rl-agent. Default is cuda:0.""" + + num_steps_per_env: int = MISSING + """The number of steps per environment per update.""" + + max_iterations: int = MISSING + """The maximum number of iterations.""" + + empirical_normalization: bool = MISSING + """Whether to use empirical normalization.""" + + policy: RslRlPpoActorCriticCfg | RslRlDistillationStudentTeacherCfg = MISSING + """The policy configuration.""" + + algorithm: RslRlPpoAlgorithmCfg | RslRlDistillationAlgorithmCfg = MISSING + """The algorithm configuration.""" + + clip_actions: float | None = None + """The clipping value for actions. If ``None``, then no clipping is done. + + .. note:: + This clipping is performed inside the :class:`RslRlVecEnvWrapper` wrapper. + """ + + save_interval: int = MISSING + """The number of iterations between saves.""" + + experiment_name: str = MISSING + """The experiment name.""" + + run_name: str = "" + """The run name. Default is empty string. + + The name of the run directory is typically the time-stamp at execution. If the run name is not empty, + then it is appended to the run directory's name, i.e. the logging directory's name will become + ``{time-stamp}_{run_name}``. + """ + + logger: Literal["tensorboard", "neptune", "wandb"] = "tensorboard" + """The logger to use. Default is tensorboard.""" + + neptune_project: str = "isaaclab" + """The neptune project name. Default is "isaaclab".""" + + wandb_project: str = "isaaclab" + """The wandb project name. Default is "isaaclab".""" + + resume: bool = False + """Whether to resume. Default is False.""" + + load_run: str = ".*" + """The run directory to load. Default is ".*" (all). + + If regex expression, the latest (alphabetical order) matching run will be loaded. + """ + + load_checkpoint: str = "model_.*.pt" + """The checkpoint file to load. Default is ``"model_.*.pt"`` (all). + + If regex expression, the latest (alphabetical order) matching file will be loaded. + """ diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rnd_cfg.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rnd_cfg.py new file mode 100644 index 00000000000..41f1d50858a --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/rnd_cfg.py @@ -0,0 +1,104 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class RslRlRndCfg: + """Configuration for the Random Network Distillation (RND) module. + + For more information, please check the work from :cite:`schwarke2023curiosity`. + """ + + @configclass + class WeightScheduleCfg: + """Configuration for the weight schedule.""" + + mode: str = "constant" + """The type of weight schedule. Default is "constant".""" + + @configclass + class LinearWeightScheduleCfg(WeightScheduleCfg): + """Configuration for the linear weight schedule. + + This schedule decays the weight linearly from the initial value to the final value + between :attr:`initial_step` and before :attr:`final_step`. + """ + + mode: str = "linear" + + final_value: float = MISSING + """The final value of the weight parameter.""" + + initial_step: int = MISSING + """The initial step of the weight schedule. + + For steps before this step, the weight is the initial value specified in :attr:`RslRlRndCfg.weight`. + """ + + final_step: int = MISSING + """The final step of the weight schedule. + + For steps after this step, the weight is the final value specified in :attr:`final_value`. + """ + + @configclass + class StepWeightScheduleCfg(WeightScheduleCfg): + """Configuration for the step weight schedule. + + This schedule sets the weight to the value specified in :attr:`final_value` at step :attr:`final_step`. + """ + + mode: str = "step" + + final_step: int = MISSING + """The final step of the weight schedule. + + For steps after this step, the weight is the value specified in :attr:`final_value`. + """ + + final_value: float = MISSING + """The final value of the weight parameter.""" + + weight: float = 0.0 + """The weight for the RND reward (also known as intrinsic reward). Default is 0.0. + + Similar to other reward terms, the RND reward is scaled by this weight. + """ + + weight_schedule: WeightScheduleCfg | None = None + """The weight schedule for the RND reward. Default is None, which means the weight is constant.""" + + reward_normalization: bool = False + """Whether to normalize the RND reward. Default is False.""" + + state_normalization: bool = False + """Whether to normalize the RND state. Default is False.""" + + learning_rate: float = 1e-3 + """The learning rate for the RND module. Default is 1e-3.""" + + num_outputs: int = 1 + """The number of outputs for the RND module. Default is 1.""" + + predictor_hidden_dims: list[int] = [-1] + """The hidden dimensions for the RND predictor network. Default is [-1]. + + If the list contains -1, then the hidden dimensions are the same as the input dimensions. + """ + + target_hidden_dims: list[int] = [-1] + """The hidden dimensions for the RND target network. Default is [-1]. + + If the list contains -1, then the hidden dimensions are the same as the input dimensions. + """ diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/symmetry_cfg.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/symmetry_cfg.py new file mode 100644 index 00000000000..efd1a69cf0e --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/symmetry_cfg.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class RslRlSymmetryCfg: + """Configuration for the symmetry-augmentation in the training. + + When :meth:`use_data_augmentation` is True, the :meth:`data_augmentation_func` is used to generate + augmented observations and actions. These are then used to train the model. + + When :meth:`use_mirror_loss` is True, the :meth:`mirror_loss_coeff` is used to weight the + symmetry-mirror loss. This loss is directly added to the agent's loss function. + + If both :meth:`use_data_augmentation` and :meth:`use_mirror_loss` are False, then no symmetry-based + training is enabled. However, the :meth:`data_augmentation_func` is called to compute and log + symmetry metrics. This is useful for performing ablations. + + For more information, please check the work from :cite:`mittal2024symmetry`. + """ + + use_data_augmentation: bool = False + """Whether to use symmetry-based data augmentation. Default is False.""" + + use_mirror_loss: bool = False + """Whether to use the symmetry-augmentation loss. Default is False.""" + + data_augmentation_func: callable = MISSING + """The symmetry data augmentation function. + + The function signature should be as follows: + + Args: + + env (VecEnv): The environment object. This is used to access the environment's properties. + obs (torch.Tensor | None): The observation tensor. If None, the observation is not used. + action (torch.Tensor | None): The action tensor. If None, the action is not used. + obs_type (str): The name of the observation type. Defaults to "policy". + This is useful when handling augmentation for different observation groups. + + Returns: + A tuple containing the augmented observation and action tensors. The tensors can be None, + if their respective inputs are None. + """ + + mirror_loss_coeff: float = 0.0 + """The weight for the symmetry-mirror loss. Default is 0.0.""" diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/vecenv_wrapper.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/vecenv_wrapper.py new file mode 100644 index 00000000000..ffd746e3dc4 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/rsl_rl/vecenv_wrapper.py @@ -0,0 +1,214 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym +import torch + +from rsl_rl.env import VecEnv + +from isaaclab.envs import DirectRLEnv, ManagerBasedRLEnv + + +class RslRlVecEnvWrapper(VecEnv): + """Wraps around Isaac Lab environment for RSL-RL library + + To use asymmetric actor-critic, the environment instance must have the attributes :attr:`num_privileged_obs` (int). + This is used by the learning agent to allocate buffers in the trajectory memory. Additionally, the returned + observations should have the key "critic" which corresponds to the privileged observations. Since this is + optional for some environments, the wrapper checks if these attributes exist. If they don't then the wrapper + defaults to zero as number of privileged observations. + + .. caution:: + + This class must be the last wrapper in the wrapper chain. This is because the wrapper does not follow + the :class:`gym.Wrapper` interface. Any subsequent wrappers will need to be modified to work with this + wrapper. + + Reference: + https://github.com/leggedrobotics/rsl_rl/blob/master/rsl_rl/env/vec_env.py + """ + + def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv, clip_actions: float | None = None): + """Initializes the wrapper. + + Note: + The wrapper calls :meth:`reset` at the start since the RSL-RL runner does not call reset. + + Args: + env: The environment to wrap around. + clip_actions: The clipping value for actions. If ``None``, then no clipping is done. + + Raises: + ValueError: When the environment is not an instance of :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. + """ + # check that input is valid + if not isinstance(env.unwrapped, ManagerBasedRLEnv) and not isinstance(env.unwrapped, DirectRLEnv): + raise ValueError( + "The environment must be inherited from ManagerBasedRLEnv or DirectRLEnv. Environment type:" + f" {type(env)}" + ) + # initialize the wrapper + self.env = env + self.clip_actions = clip_actions + + # store information required by wrapper + self.num_envs = self.unwrapped.num_envs + self.device = self.unwrapped.device + self.max_episode_length = self.unwrapped.max_episode_length + + # obtain dimensions of the environment + if hasattr(self.unwrapped, "action_manager"): + self.num_actions = self.unwrapped.action_manager.total_action_dim + else: + self.num_actions = gym.spaces.flatdim(self.unwrapped.single_action_space) + if hasattr(self.unwrapped, "observation_manager"): + self.num_obs = self.unwrapped.observation_manager.group_obs_dim["policy"][0] + else: + self.num_obs = gym.spaces.flatdim(self.unwrapped.single_observation_space["policy"]) + # -- privileged observations + if ( + hasattr(self.unwrapped, "observation_manager") + and "critic" in self.unwrapped.observation_manager.group_obs_dim + ): + self.num_privileged_obs = self.unwrapped.observation_manager.group_obs_dim["critic"][0] + elif hasattr(self.unwrapped, "num_states") and "critic" in self.unwrapped.single_observation_space: + self.num_privileged_obs = gym.spaces.flatdim(self.unwrapped.single_observation_space["critic"]) + else: + self.num_privileged_obs = 0 + + # modify the action space to the clip range + self._modify_action_space() + + # reset at the start since the RSL-RL runner does not call reset + self.env.reset() + + def __str__(self): + """Returns the wrapper name and the :attr:`env` representation string.""" + return f"<{type(self).__name__}{self.env}>" + + def __repr__(self): + """Returns the string representation of the wrapper.""" + return str(self) + + """ + Properties -- Gym.Wrapper + """ + + @property + def cfg(self) -> object: + """Returns the configuration class instance of the environment.""" + return self.unwrapped.cfg + + @property + def render_mode(self) -> str | None: + """Returns the :attr:`Env` :attr:`render_mode`.""" + return self.env.render_mode + + @property + def observation_space(self) -> gym.Space: + """Returns the :attr:`Env` :attr:`observation_space`.""" + return self.env.observation_space + + @property + def action_space(self) -> gym.Space: + """Returns the :attr:`Env` :attr:`action_space`.""" + return self.env.action_space + + @classmethod + def class_name(cls) -> str: + """Returns the class name of the wrapper.""" + return cls.__name__ + + @property + def unwrapped(self) -> ManagerBasedRLEnv | DirectRLEnv: + """Returns the base environment of the wrapper. + + This will be the bare :class:`gymnasium.Env` environment, underneath all layers of wrappers. + """ + return self.env.unwrapped + + """ + Properties + """ + + def get_observations(self) -> tuple[torch.Tensor, dict]: + """Returns the current observations of the environment.""" + if hasattr(self.unwrapped, "observation_manager"): + obs_dict = self.unwrapped.observation_manager.compute() + else: + obs_dict = self.unwrapped._get_observations() + return obs_dict["policy"], {"observations": obs_dict} + + @property + def episode_length_buf(self) -> torch.Tensor: + """The episode length buffer.""" + return self.unwrapped.episode_length_buf + + @episode_length_buf.setter + def episode_length_buf(self, value: torch.Tensor): + """Set the episode length buffer. + + Note: + This is needed to perform random initialization of episode lengths in RSL-RL. + """ + self.unwrapped.episode_length_buf = value + + """ + Operations - MDP + """ + + def seed(self, seed: int = -1) -> int: # noqa: D102 + return self.unwrapped.seed(seed) + + def reset(self) -> tuple[torch.Tensor, dict]: # noqa: D102 + # reset the environment + obs_dict, _ = self.env.reset() + # return observations + return obs_dict["policy"], {"observations": obs_dict} + + def step(self, actions: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, dict]: + # clip actions + if self.clip_actions is not None: + actions = torch.clamp(actions, -self.clip_actions, self.clip_actions) + # record step information + obs_dict, rew, terminated, truncated, extras = self.env.step(actions) + # compute dones for compatibility with RSL-RL + dones = (terminated | truncated).to(dtype=torch.long) + # move extra observations to the extras dict + obs = obs_dict["policy"] + extras["observations"] = obs_dict + # move time out information to the extras dict + # this is only needed for infinite horizon tasks + if not self.unwrapped.cfg.is_finite_horizon: + extras["time_outs"] = truncated + + # return the step information + return obs, rew, dones, extras + + def close(self): # noqa: D102 + return self.env.close() + + """ + Helper functions + """ + + def _modify_action_space(self): + """Modifies the action space to the clip range.""" + if self.clip_actions is None: + return + + # modify the action space to the clip range + # note: this is only possible for the box action space. we need to change it in the future for other action spaces. + self.env.unwrapped.single_action_space = gym.spaces.Box( + low=-self.clip_actions, high=self.clip_actions, shape=(self.num_actions,) + ) + self.env.unwrapped.action_space = gym.vector.utils.batch_space( + self.env.unwrapped.single_action_space, self.num_envs + ) diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/sb3.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/sb3.py new file mode 100644 index 00000000000..2f5912328e8 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/sb3.py @@ -0,0 +1,353 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an environment instance to Stable-Baselines3 vectorized environment. + +The following example shows how to wrap an environment for Stable-Baselines3: + +.. code-block:: python + + from isaaclab_rl.sb3 import Sb3VecEnvWrapper + + env = Sb3VecEnvWrapper(env) + +""" + +# needed to import for allowing type-hinting: torch.Tensor | dict[str, torch.Tensor] +from __future__ import annotations + +import gymnasium as gym +import numpy as np +import torch +import torch.nn as nn # noqa: F401 +from typing import Any + +from stable_baselines3.common.utils import constant_fn +from stable_baselines3.common.vec_env.base_vec_env import VecEnv, VecEnvObs, VecEnvStepReturn + +from isaaclab.envs import DirectRLEnv, ManagerBasedRLEnv + +""" +Configuration Parser. +""" + + +def process_sb3_cfg(cfg: dict) -> dict: + """Convert simple YAML types to Stable-Baselines classes/components. + + Args: + cfg: A configuration dictionary. + + Returns: + A dictionary containing the converted configuration. + + Reference: + https://github.com/DLR-RM/rl-baselines3-zoo/blob/0e5eb145faefa33e7d79c7f8c179788574b20da5/utils/exp_manager.py#L358 + """ + + def update_dict(hyperparams: dict[str, Any]) -> dict[str, Any]: + for key, value in hyperparams.items(): + if isinstance(value, dict): + update_dict(value) + else: + if key in ["policy_kwargs", "replay_buffer_class", "replay_buffer_kwargs"]: + hyperparams[key] = eval(value) + elif key in ["learning_rate", "clip_range", "clip_range_vf", "delta_std"]: + if isinstance(value, str): + _, initial_value = value.split("_") + initial_value = float(initial_value) + hyperparams[key] = lambda progress_remaining: progress_remaining * initial_value + elif isinstance(value, (float, int)): + # Negative value: ignore (ex: for clipping) + if value < 0: + continue + hyperparams[key] = constant_fn(float(value)) + else: + raise ValueError(f"Invalid value for {key}: {hyperparams[key]}") + + return hyperparams + + # parse agent configuration and convert to classes + return update_dict(cfg) + + +""" +Vectorized environment wrapper. +""" + + +class Sb3VecEnvWrapper(VecEnv): + """Wraps around Isaac Lab environment for Stable Baselines3. + + Isaac Sim internally implements a vectorized environment. However, since it is + still considered a single environment instance, Stable Baselines tries to wrap + around it using the :class:`DummyVecEnv`. This is only done if the environment + is not inheriting from their :class:`VecEnv`. Thus, this class thinly wraps + over the environment from :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. + + Note: + While Stable-Baselines3 supports Gym 0.26+ API, their vectorized environment + still uses the old API (i.e. it is closer to Gym 0.21). Thus, we implement + the old API for the vectorized environment. + + We also add monitoring functionality that computes the un-discounted episode + return and length. This information is added to the info dicts under key `episode`. + + In contrast to the Isaac Lab environment, stable-baselines expect the following: + + 1. numpy datatype for MDP signals + 2. a list of info dicts for each sub-environment (instead of a dict) + 3. when environment has terminated, the observations from the environment should correspond + to the one after reset. The "real" final observation is passed using the info dicts + under the key ``terminal_observation``. + + .. warning:: + + By the nature of physics stepping in Isaac Sim, it is not possible to forward the + simulation buffers without performing a physics step. Thus, reset is performed + inside the :meth:`step()` function after the actual physics step is taken. + Thus, the returned observations for terminated environments is the one after the reset. + + .. caution:: + + This class must be the last wrapper in the wrapper chain. This is because the wrapper does not follow + the :class:`gym.Wrapper` interface. Any subsequent wrappers will need to be modified to work with this + wrapper. + + Reference: + + 1. https://stable-baselines3.readthedocs.io/en/master/guide/vec_envs.html + 2. https://stable-baselines3.readthedocs.io/en/master/common/monitor.html + + """ + + def __init__(self, env: ManagerBasedRLEnv | DirectRLEnv): + """Initialize the wrapper. + + Args: + env: The environment to wrap around. + + Raises: + ValueError: When the environment is not an instance of :class:`ManagerBasedRLEnv` or :class:`DirectRLEnv`. + """ + # check that input is valid + if not isinstance(env.unwrapped, ManagerBasedRLEnv) and not isinstance(env.unwrapped, DirectRLEnv): + raise ValueError( + "The environment must be inherited from ManagerBasedRLEnv or DirectRLEnv. Environment type:" + f" {type(env)}" + ) + # initialize the wrapper + self.env = env + # collect common information + self.num_envs = self.unwrapped.num_envs + self.sim_device = self.unwrapped.device + self.render_mode = self.unwrapped.render_mode + + # obtain gym spaces + # note: stable-baselines3 does not like when we have unbounded action space so + # we set it to some high value here. Maybe this is not general but something to think about. + observation_space = self.unwrapped.single_observation_space["policy"] + action_space = self.unwrapped.single_action_space + if isinstance(action_space, gym.spaces.Box) and not action_space.is_bounded("both"): + action_space = gym.spaces.Box(low=-100, high=100, shape=action_space.shape) + + # initialize vec-env + VecEnv.__init__(self, self.num_envs, observation_space, action_space) + # add buffer for logging episodic information + self._ep_rew_buf = torch.zeros(self.num_envs, device=self.sim_device) + self._ep_len_buf = torch.zeros(self.num_envs, device=self.sim_device) + + def __str__(self): + """Returns the wrapper name and the :attr:`env` representation string.""" + return f"<{type(self).__name__}{self.env}>" + + def __repr__(self): + """Returns the string representation of the wrapper.""" + return str(self) + + """ + Properties -- Gym.Wrapper + """ + + @classmethod + def class_name(cls) -> str: + """Returns the class name of the wrapper.""" + return cls.__name__ + + @property + def unwrapped(self) -> ManagerBasedRLEnv | DirectRLEnv: + """Returns the base environment of the wrapper. + + This will be the bare :class:`gymnasium.Env` environment, underneath all layers of wrappers. + """ + return self.env.unwrapped + + """ + Properties + """ + + def get_episode_rewards(self) -> list[float]: + """Returns the rewards of all the episodes.""" + return self._ep_rew_buf.cpu().tolist() + + def get_episode_lengths(self) -> list[int]: + """Returns the number of time-steps of all the episodes.""" + return self._ep_len_buf.cpu().tolist() + + """ + Operations - MDP + """ + + def seed(self, seed: int | None = None) -> list[int | None]: # noqa: D102 + return [self.unwrapped.seed(seed)] * self.unwrapped.num_envs + + def reset(self) -> VecEnvObs: # noqa: D102 + obs_dict, _ = self.env.reset() + # reset episodic information buffers + self._ep_rew_buf.zero_() + self._ep_len_buf.zero_() + # convert data types to numpy depending on backend + return self._process_obs(obs_dict) + + def step_async(self, actions): # noqa: D102 + # convert input to numpy array + if not isinstance(actions, torch.Tensor): + actions = np.asarray(actions) + actions = torch.from_numpy(actions).to(device=self.sim_device, dtype=torch.float32) + else: + actions = actions.to(device=self.sim_device, dtype=torch.float32) + # convert to tensor + self._async_actions = actions + + def step_wait(self) -> VecEnvStepReturn: # noqa: D102 + # record step information + obs_dict, rew, terminated, truncated, extras = self.env.step(self._async_actions) + # update episode un-discounted return and length + self._ep_rew_buf += rew + self._ep_len_buf += 1 + # compute reset ids + dones = terminated | truncated + reset_ids = (dones > 0).nonzero(as_tuple=False) + + # convert data types to numpy depending on backend + # note: ManagerBasedRLEnv uses torch backend (by default). + obs = self._process_obs(obs_dict) + rew = rew.detach().cpu().numpy() + terminated = terminated.detach().cpu().numpy() + truncated = truncated.detach().cpu().numpy() + dones = dones.detach().cpu().numpy() + # convert extra information to list of dicts + infos = self._process_extras(obs, terminated, truncated, extras, reset_ids) + + # reset info for terminated environments + self._ep_rew_buf[reset_ids] = 0 + self._ep_len_buf[reset_ids] = 0 + + return obs, rew, dones, infos + + def close(self): # noqa: D102 + self.env.close() + + def get_attr(self, attr_name, indices=None): # noqa: D102 + # resolve indices + if indices is None: + indices = slice(None) + num_indices = self.num_envs + else: + num_indices = len(indices) + # obtain attribute value + attr_val = getattr(self.env, attr_name) + # return the value + if not isinstance(attr_val, torch.Tensor): + return [attr_val] * num_indices + else: + return attr_val[indices].detach().cpu().numpy() + + def set_attr(self, attr_name, value, indices=None): # noqa: D102 + raise NotImplementedError("Setting attributes is not supported.") + + def env_method(self, method_name: str, *method_args, indices=None, **method_kwargs): # noqa: D102 + if method_name == "render": + # gymnasium does not support changing render mode at runtime + return self.env.render() + else: + # this isn't properly implemented but it is not necessary. + # mostly done for completeness. + env_method = getattr(self.env, method_name) + return env_method(*method_args, indices=indices, **method_kwargs) + + def env_is_wrapped(self, wrapper_class, indices=None): # noqa: D102 + raise NotImplementedError("Checking if environment is wrapped is not supported.") + + def get_images(self): # noqa: D102 + raise NotImplementedError("Getting images is not supported.") + + """ + Helper functions. + """ + + def _process_obs(self, obs_dict: torch.Tensor | dict[str, torch.Tensor]) -> np.ndarray | dict[str, np.ndarray]: + """Convert observations into NumPy data type.""" + # Sb3 doesn't support asymmetric observation spaces, so we only use "policy" + obs = obs_dict["policy"] + # note: ManagerBasedRLEnv uses torch backend (by default). + if isinstance(obs, dict): + for key, value in obs.items(): + obs[key] = value.detach().cpu().numpy() + elif isinstance(obs, torch.Tensor): + obs = obs.detach().cpu().numpy() + else: + raise NotImplementedError(f"Unsupported data type: {type(obs)}") + return obs + + def _process_extras( + self, obs: np.ndarray, terminated: np.ndarray, truncated: np.ndarray, extras: dict, reset_ids: np.ndarray + ) -> list[dict[str, Any]]: + """Convert miscellaneous information into dictionary for each sub-environment.""" + # create empty list of dictionaries to fill + infos: list[dict[str, Any]] = [dict.fromkeys(extras.keys()) for _ in range(self.num_envs)] + # fill-in information for each sub-environment + # note: This loop becomes slow when number of environments is large. + for idx in range(self.num_envs): + # fill-in episode monitoring info + if idx in reset_ids: + infos[idx]["episode"] = dict() + infos[idx]["episode"]["r"] = float(self._ep_rew_buf[idx]) + infos[idx]["episode"]["l"] = float(self._ep_len_buf[idx]) + else: + infos[idx]["episode"] = None + # fill-in bootstrap information + infos[idx]["TimeLimit.truncated"] = truncated[idx] and not terminated[idx] + # fill-in information from extras + for key, value in extras.items(): + # 1. remap extra episodes information safely + # 2. for others just store their values + if key == "log": + # only log this data for episodes that are terminated + if infos[idx]["episode"] is not None: + for sub_key, sub_value in value.items(): + infos[idx]["episode"][sub_key] = sub_value + else: + infos[idx][key] = value[idx] + # add information about terminal observation separately + if idx in reset_ids: + # extract terminal observations + if isinstance(obs, dict): + terminal_obs = dict.fromkeys(obs.keys()) + for key, value in obs.items(): + terminal_obs[key] = value[idx] + else: + terminal_obs = obs[idx] + # add info to dict + infos[idx]["terminal_observation"] = terminal_obs + else: + infos[idx]["terminal_observation"] = None + # return list of dictionaries + return infos diff --git a/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/skrl.py b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/skrl.py new file mode 100644 index 00000000000..48776aa9a92 --- /dev/null +++ b/install/isaaclab_rl/lib/python3.10/site-packages/isaaclab_rl/skrl.py @@ -0,0 +1,91 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrapper to configure an environment instance to skrl environment. + +The following example shows how to wrap an environment for skrl: + +.. code-block:: python + + from isaaclab_rl.skrl import SkrlVecEnvWrapper + + env = SkrlVecEnvWrapper(env, ml_framework="torch") # or ml_framework="jax" + +Or, equivalently, by directly calling the skrl library API as follows: + +.. code-block:: python + + from skrl.envs.torch.wrappers import wrap_env # for PyTorch, or... + from skrl.envs.jax.wrappers import wrap_env # for JAX + + env = wrap_env(env, wrapper="isaaclab") + +""" + +# needed to import for type hinting: Agent | list[Agent] +from __future__ import annotations + +from typing import Literal + +from isaaclab.envs import DirectMARLEnv, DirectRLEnv, ManagerBasedRLEnv + +""" +Vectorized environment wrapper. +""" + + +def SkrlVecEnvWrapper( + env: ManagerBasedRLEnv | DirectRLEnv | DirectMARLEnv, + ml_framework: Literal["torch", "jax", "jax-numpy"] = "torch", + wrapper: Literal["auto", "isaaclab", "isaaclab-single-agent", "isaaclab-multi-agent"] = "isaaclab", +): + """Wraps around Isaac Lab environment for skrl. + + This function wraps around the Isaac Lab environment. Since the wrapping + functionality is defined within the skrl library itself, this implementation + is maintained for compatibility with the structure of the extension that contains it. + Internally it calls the :func:`wrap_env` from the skrl library API. + + Args: + env: The environment to wrap around. + ml_framework: The ML framework to use for the wrapper. Defaults to "torch". + wrapper: The wrapper to use. Defaults to "isaaclab": leave it to skrl to determine if the environment + will be wrapped as single-agent or multi-agent. + + Raises: + ValueError: When the environment is not an instance of any Isaac Lab environment interface. + ValueError: If the specified ML framework is not valid. + + Reference: + https://skrl.readthedocs.io/en/latest/api/envs/wrapping.html + """ + # check that input is valid + if ( + not isinstance(env.unwrapped, ManagerBasedRLEnv) + and not isinstance(env.unwrapped, DirectRLEnv) + and not isinstance(env.unwrapped, DirectMARLEnv) + ): + raise ValueError( + "The environment must be inherited from ManagerBasedRLEnv, DirectRLEnv or DirectMARLEnv. Environment type:" + f" {type(env)}" + ) + + # import statements according to the ML framework + if ml_framework.startswith("torch"): + from skrl.envs.wrappers.torch import wrap_env + elif ml_framework.startswith("jax"): + from skrl.envs.wrappers.jax import wrap_env + else: + ValueError( + f"Invalid ML framework for skrl: {ml_framework}. Available options are: 'torch', 'jax' or 'jax-numpy'" + ) + + # wrap and return the environment + return wrap_env(env, wrapper) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/__init__.py new file mode 100644 index 00000000000..4d364360069 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/__init__.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package containing task implementations for various robotic environments.""" + +import os +import toml + +# Conveniences to other module directories via relative paths +ISAACLAB_TASKS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +"""Path to the extension source directory.""" + +ISAACLAB_TASKS_METADATA = toml.load(os.path.join(ISAACLAB_TASKS_EXT_DIR, "config", "extension.toml")) +"""Extension metadata dictionary parsed from the extension.toml file.""" + +# Configure the module-level variables +__version__ = ISAACLAB_TASKS_METADATA["package"]["version"] + +## +# Register Gym environments. +## + +from .utils import import_packages + +# The blacklist is used to prevent importing configs from sub-packages +# TODO(@ashwinvk): Remove pick_place from the blacklist once pinocchio from Isaac Sim is compatibility +_BLACKLIST_PKGS = ["utils", ".mdp", "pick_place"] +# Import all configs in this package +import_packages(__name__, _BLACKLIST_PKGS) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/__init__.py new file mode 100644 index 00000000000..ea634a79459 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Direct workflow environments. +""" + +import gymnasium as gym diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/__init__.py new file mode 100644 index 00000000000..884aff3577a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/__init__.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Allegro Inhand Manipulation environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +inhand_task_entry = "isaaclab_tasks.direct.inhand_manipulation" + +gym.register( + id="Isaac-Repose-Cube-Allegro-Direct-v0", + entry_point=f"{inhand_task_entry}.inhand_manipulation_env:InHandManipulationEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.allegro_hand_env_cfg:AllegroHandEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AllegroHandPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..df7e8b4f715 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,86 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [1024, 512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: allegro_hand + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 5000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..fd85a9a8fdb --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AllegroHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 10000 + save_interval = 250 + experiment_name = "allegro_hand" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[1024, 512, 256, 128], + critic_hidden_dims=[1024, 512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.016, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..ca8c6299351 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [1024, 512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "allegro_hand" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 80000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py new file mode 100644 index 00000000000..c8009fe0430 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/allegro_hand/allegro_hand_env_cfg.py @@ -0,0 +1,124 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + + +@configclass +class AllegroHandEnvCfg(DirectRLEnvCfg): + # env + decimation = 4 + episode_length_s = 10.0 + action_space = 16 + observation_space = 124 # (full) + state_space = 0 + asymmetric_obs = False + obs_type = "full" + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 120, + render_interval=decimation, + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + physx=PhysxCfg( + bounce_threshold_velocity=0.2, + ), + ) + # robot + robot_cfg: ArticulationCfg = ALLEGRO_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot") + + actuated_joint_names = [ + "index_joint_0", + "middle_joint_0", + "ring_joint_0", + "thumb_joint_0", + "index_joint_1", + "index_joint_2", + "index_joint_3", + "middle_joint_1", + "middle_joint_2", + "middle_joint_3", + "ring_joint_1", + "ring_joint_2", + "ring_joint_3", + "thumb_joint_1", + "thumb_joint_2", + "thumb_joint_3", + ] + fingertip_body_names = [ + "index_link_3", + "middle_link_3", + "ring_link_3", + "thumb_link_3", + ] + + # in-hand object + object_cfg: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + mass_props=sim_utils.MassPropertiesCfg(density=400.0), + scale=(1.2, 1.2, 1.2), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.17, 0.56), rot=(1.0, 0.0, 0.0, 0.0)), + ) + # goal object + goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(1.2, 1.2, 1.2), + ) + }, + ) + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=8192, env_spacing=0.75, replicate_physics=True) + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # reward scales + dist_reward_scale = -10.0 + rot_reward_scale = 1.0 + rot_eps = 0.1 + action_penalty_scale = -0.0002 + reach_goal_bonus = 250 + fall_penalty = 0 + fall_dist = 0.24 + vel_obs_scale = 0.2 + success_tolerance = 0.2 + max_consecutive_success = 0 + av_factor = 0.1 + act_moving_average = 1.0 + force_torque_obs_scale = 10.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/__init__.py new file mode 100644 index 00000000000..451cdfe8fb5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Ant locomotion environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Ant-Direct-v0", + entry_point=f"{__name__}.ant_env:AntEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ant_env:AntEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AntPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..7c1c514c298 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: ant_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 32768 + mini_epochs: 4 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..6869fc41f65 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 32 + max_iterations = 1000 + save_interval = 50 + experiment_name = "ant_direct" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[400, 200, 100], + critic_hidden_dims=[400, 200, 100], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..5ebf90cb5a4 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 4 + mini_batches: 2 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "ant_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 8000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/ant_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/ant_env.py new file mode 100644 index 00000000000..5a9342e530d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/ant/ant_env.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab_assets.robots.ant import ANT_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv + + +@configclass +class AntEnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 15.0 + decimation = 2 + action_scale = 0.5 + action_space = 8 + observation_space = 36 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="average", + restitution_combine_mode="average", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # robot + robot: ArticulationCfg = ANT_CFG.replace(prim_path="/World/envs/env_.*/Robot") + joint_gears: list = [15, 15, 15, 15, 15, 15, 15, 15] + + heading_weight: float = 0.5 + up_weight: float = 0.1 + + energy_cost_scale: float = 0.05 + actions_cost_scale: float = 0.005 + alive_reward_scale: float = 0.5 + dof_vel_scale: float = 0.2 + + death_cost: float = -2.0 + termination_height: float = 0.31 + + angular_velocity_scale: float = 1.0 + contact_force_scale: float = 0.1 + + +class AntEnv(LocomotionEnv): + cfg: AntEnvCfg + + def __init__(self, cfg: AntEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/__init__.py new file mode 100644 index 00000000000..dd86d752d25 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/__init__.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Ant locomotion environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Anymal-C-Direct-v0", + entry_point=f"{__name__}.anymal_c_env:AnymalCEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.anymal_c_env_cfg:AnymalCFlatEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_flat_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-C-Direct-v0", + entry_point=f"{__name__}.anymal_c_env:AnymalCEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.anymal_c_env_cfg:AnymalCRoughEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..4bb7435f094 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_flat_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [128, 128, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: anymal_c_flat_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: False + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 20000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.005 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2.0 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..1995015eb79 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rl_games_rough_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: anymal_c_rough_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: False + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 20000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.005 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2.0 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..50109c922de --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AnymalCFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 500 + save_interval = 50 + experiment_name = "anymal_c_flat_direct" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[128, 128, 128], + critic_hidden_dims=[128, 128, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "anymal_c_rough_direct" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..aa5828e7686 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_c_flat_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..1c72b8fb13d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_c_rough_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env.py new file mode 100644 index 00000000000..7d4aec17e6d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env.py @@ -0,0 +1,199 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.envs import DirectRLEnv +from isaaclab.sensors import ContactSensor, RayCaster + +from .anymal_c_env_cfg import AnymalCFlatEnvCfg, AnymalCRoughEnvCfg + + +class AnymalCEnv(DirectRLEnv): + cfg: AnymalCFlatEnvCfg | AnymalCRoughEnvCfg + + def __init__(self, cfg: AnymalCFlatEnvCfg | AnymalCRoughEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + # Joint position command (deviation from default joint positions) + self._actions = torch.zeros(self.num_envs, gym.spaces.flatdim(self.single_action_space), device=self.device) + self._previous_actions = torch.zeros( + self.num_envs, gym.spaces.flatdim(self.single_action_space), device=self.device + ) + + # X/Y linear velocity and yaw angular velocity commands + self._commands = torch.zeros(self.num_envs, 3, device=self.device) + + # Logging + self._episode_sums = { + key: torch.zeros(self.num_envs, dtype=torch.float, device=self.device) + for key in [ + "track_lin_vel_xy_exp", + "track_ang_vel_z_exp", + "lin_vel_z_l2", + "ang_vel_xy_l2", + "dof_torques_l2", + "dof_acc_l2", + "action_rate_l2", + "feet_air_time", + "undesired_contacts", + "flat_orientation_l2", + ] + } + # Get specific body indices + self._base_id, _ = self._contact_sensor.find_bodies("base") + self._feet_ids, _ = self._contact_sensor.find_bodies(".*FOOT") + self._undesired_contact_body_ids, _ = self._contact_sensor.find_bodies(".*THIGH") + + def _setup_scene(self): + self._robot = Articulation(self.cfg.robot) + self.scene.articulations["robot"] = self._robot + self._contact_sensor = ContactSensor(self.cfg.contact_sensor) + self.scene.sensors["contact_sensor"] = self._contact_sensor + if isinstance(self.cfg, AnymalCRoughEnvCfg): + # we add a height scanner for perceptive locomotion + self._height_scanner = RayCaster(self.cfg.height_scanner) + self.scene.sensors["height_scanner"] = self._height_scanner + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self._terrain = self.cfg.terrain.class_type(self.cfg.terrain) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor): + self._actions = actions.clone() + self._processed_actions = self.cfg.action_scale * self._actions + self._robot.data.default_joint_pos + + def _apply_action(self): + self._robot.set_joint_position_target(self._processed_actions) + + def _get_observations(self) -> dict: + self._previous_actions = self._actions.clone() + height_data = None + if isinstance(self.cfg, AnymalCRoughEnvCfg): + height_data = ( + self._height_scanner.data.pos_w[:, 2].unsqueeze(1) - self._height_scanner.data.ray_hits_w[..., 2] - 0.5 + ).clip(-1.0, 1.0) + obs = torch.cat( + [ + tensor + for tensor in ( + self._robot.data.root_lin_vel_b, + self._robot.data.root_ang_vel_b, + self._robot.data.projected_gravity_b, + self._commands, + self._robot.data.joint_pos - self._robot.data.default_joint_pos, + self._robot.data.joint_vel, + height_data, + self._actions, + ) + if tensor is not None + ], + dim=-1, + ) + observations = {"policy": obs} + return observations + + def _get_rewards(self) -> torch.Tensor: + # linear velocity tracking + lin_vel_error = torch.sum(torch.square(self._commands[:, :2] - self._robot.data.root_lin_vel_b[:, :2]), dim=1) + lin_vel_error_mapped = torch.exp(-lin_vel_error / 0.25) + # yaw rate tracking + yaw_rate_error = torch.square(self._commands[:, 2] - self._robot.data.root_ang_vel_b[:, 2]) + yaw_rate_error_mapped = torch.exp(-yaw_rate_error / 0.25) + # z velocity tracking + z_vel_error = torch.square(self._robot.data.root_lin_vel_b[:, 2]) + # angular velocity x/y + ang_vel_error = torch.sum(torch.square(self._robot.data.root_ang_vel_b[:, :2]), dim=1) + # joint torques + joint_torques = torch.sum(torch.square(self._robot.data.applied_torque), dim=1) + # joint acceleration + joint_accel = torch.sum(torch.square(self._robot.data.joint_acc), dim=1) + # action rate + action_rate = torch.sum(torch.square(self._actions - self._previous_actions), dim=1) + # feet air time + first_contact = self._contact_sensor.compute_first_contact(self.step_dt)[:, self._feet_ids] + last_air_time = self._contact_sensor.data.last_air_time[:, self._feet_ids] + air_time = torch.sum((last_air_time - 0.5) * first_contact, dim=1) * ( + torch.norm(self._commands[:, :2], dim=1) > 0.1 + ) + # undesired contacts + net_contact_forces = self._contact_sensor.data.net_forces_w_history + is_contact = ( + torch.max(torch.norm(net_contact_forces[:, :, self._undesired_contact_body_ids], dim=-1), dim=1)[0] > 1.0 + ) + contacts = torch.sum(is_contact, dim=1) + # flat orientation + flat_orientation = torch.sum(torch.square(self._robot.data.projected_gravity_b[:, :2]), dim=1) + + rewards = { + "track_lin_vel_xy_exp": lin_vel_error_mapped * self.cfg.lin_vel_reward_scale * self.step_dt, + "track_ang_vel_z_exp": yaw_rate_error_mapped * self.cfg.yaw_rate_reward_scale * self.step_dt, + "lin_vel_z_l2": z_vel_error * self.cfg.z_vel_reward_scale * self.step_dt, + "ang_vel_xy_l2": ang_vel_error * self.cfg.ang_vel_reward_scale * self.step_dt, + "dof_torques_l2": joint_torques * self.cfg.joint_torque_reward_scale * self.step_dt, + "dof_acc_l2": joint_accel * self.cfg.joint_accel_reward_scale * self.step_dt, + "action_rate_l2": action_rate * self.cfg.action_rate_reward_scale * self.step_dt, + "feet_air_time": air_time * self.cfg.feet_air_time_reward_scale * self.step_dt, + "undesired_contacts": contacts * self.cfg.undesired_contact_reward_scale * self.step_dt, + "flat_orientation_l2": flat_orientation * self.cfg.flat_orientation_reward_scale * self.step_dt, + } + reward = torch.sum(torch.stack(list(rewards.values())), dim=0) + # Logging + for key, value in rewards.items(): + self._episode_sums[key] += value + return reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + time_out = self.episode_length_buf >= self.max_episode_length - 1 + net_contact_forces = self._contact_sensor.data.net_forces_w_history + died = torch.any(torch.max(torch.norm(net_contact_forces[:, :, self._base_id], dim=-1), dim=1)[0] > 1.0, dim=1) + return died, time_out + + def _reset_idx(self, env_ids: torch.Tensor | None): + if env_ids is None or len(env_ids) == self.num_envs: + env_ids = self._robot._ALL_INDICES + self._robot.reset(env_ids) + super()._reset_idx(env_ids) + if len(env_ids) == self.num_envs: + # Spread out the resets to avoid spikes in training when many environments reset at a similar time + self.episode_length_buf[:] = torch.randint_like(self.episode_length_buf, high=int(self.max_episode_length)) + self._actions[env_ids] = 0.0 + self._previous_actions[env_ids] = 0.0 + # Sample new commands + self._commands[env_ids] = torch.zeros_like(self._commands[env_ids]).uniform_(-1.0, 1.0) + # Reset robot state + joint_pos = self._robot.data.default_joint_pos[env_ids] + joint_vel = self._robot.data.default_joint_vel[env_ids] + default_root_state = self._robot.data.default_root_state[env_ids] + default_root_state[:, :3] += self._terrain.env_origins[env_ids] + self._robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + # Logging + extras = dict() + for key in self._episode_sums.keys(): + episodic_sum_avg = torch.mean(self._episode_sums[key][env_ids]) + extras["Episode_Reward/" + key] = episodic_sum_avg / self.max_episode_length_s + self._episode_sums[key][env_ids] = 0.0 + self.extras["log"] = dict() + self.extras["log"].update(extras) + extras = dict() + extras["Episode_Termination/base_contact"] = torch.count_nonzero(self.reset_terminated[env_ids]).item() + extras["Episode_Termination/time_out"] = torch.count_nonzero(self.reset_time_outs[env_ids]).item() + self.extras["log"].update(extras) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py new file mode 100644 index 00000000000..b604f852da0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py @@ -0,0 +1,153 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.envs.mdp as mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg, RayCasterCfg, patterns +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass + +## +# Pre-defined configs +## +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip +from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG # isort: skip + + +@configclass +class EventCfg: + """Configuration for randomization.""" + + physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 0.8), + "dynamic_friction_range": (0.6, 0.6), + "restitution_range": (0.0, 0.0), + "num_buckets": 64, + }, + ) + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "mass_distribution_params": (-5.0, 5.0), + "operation": "add", + }, + ) + + +@configclass +class AnymalCFlatEnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 20.0 + decimation = 4 + action_scale = 0.5 + action_space = 12 + observation_space = 48 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 200, + render_interval=decimation, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + ) + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # events + events: EventCfg = EventCfg() + + # robot + robot: ArticulationCfg = ANYMAL_C_CFG.replace(prim_path="/World/envs/env_.*/Robot") + contact_sensor: ContactSensorCfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/Robot/.*", history_length=3, update_period=0.005, track_air_time=True + ) + + # reward scales + lin_vel_reward_scale = 1.0 + yaw_rate_reward_scale = 0.5 + z_vel_reward_scale = -2.0 + ang_vel_reward_scale = -0.05 + joint_torque_reward_scale = -2.5e-5 + joint_accel_reward_scale = -2.5e-7 + action_rate_reward_scale = -0.01 + feet_air_time_reward_scale = 0.5 + undesired_contact_reward_scale = -1.0 + flat_orientation_reward_scale = -5.0 + + +@configclass +class AnymalCRoughEnvCfg(AnymalCFlatEnvCfg): + # env + observation_space = 235 + + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=ROUGH_TERRAINS_CFG, + max_init_terrain_level=9, + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path="{NVIDIA_NUCLEUS_DIR}/Materials/Base/Architecture/Shingles_01.mdl", + project_uvw=True, + ), + debug_vis=False, + ) + + # we add a height scanner for perceptive locomotion + height_scanner = RayCasterCfg( + prim_path="/World/envs/env_.*/Robot/base", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + attach_yaw_only=True, + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=False, + mesh_prim_paths=["/World/ground"], + ) + + # reward scales (override from flat config) + flat_orientation_reward_scale = 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/__init__.py new file mode 100644 index 00000000000..c9bae9435e1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/__init__.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents +from .assembly_env import AssemblyEnv, AssemblyEnvCfg +from .disassembly_env import DisassemblyEnv, DisassemblyEnvCfg + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-AutoMate-Assembly-Direct-v0", + entry_point="isaaclab_tasks.direct.automate:AssemblyEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": AssemblyEnvCfg, + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-AutoMate-Disassembly-Direct-v0", + entry_point="isaaclab_tasks.direct.automate:DisassemblyEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": DisassemblyEnvCfg, + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..0fba41430dc --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,100 @@ +params: + seed: 0 + algo: + name: a2c_continuous + env: + clip_actions: 1.0 + model: + name: continuous_a2c_logstd + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + initializer: + name: default + regularizer: + name: None + + rnn: + name: lstm + units: 256 + layers: 2 + before_mlp: True + concat_input: True + layer_norm: False + load_checkpoint: False + load_path: "" + config: + name: Assembly + device: cuda:0 + full_experiment_name: test + env_name: rlgpu + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: 128 + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-4 + lr_schedule: fixed + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 20000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 300 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: False + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 4096 # batch size = num_envs * horizon_length; minibatch_size = batch_size / num_minibatches + mini_epochs: 8 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + central_value_config: + minibatch_size: 256 + mini_epochs: 4 + learning_rate: 1e-3 + lr_schedule: linear + kl_threshold: 0.016 + clip_value: True + normalize_input: True + truncate_grads: True + network: + name: actor_critic + central_value: True + + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + initializer: + name: default + regularizer: + name: None + + player: + deterministic: False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env.py new file mode 100644 index 00000000000..9aca849f8d0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env.py @@ -0,0 +1,922 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import json +import numpy as np +import os +import torch +from datetime import datetime + +import carb +import isaacsim.core.utils.torch as torch_utils +import wandb +import warp as wp + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.math import axis_angle_from_quat + +from . import automate_algo_utils as automate_algo +from . import automate_log_utils as automate_log +from . import factory_control as fc +from . import industreal_algo_utils as industreal_algo +from .assembly_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, AssemblyEnvCfg +from .soft_dtw_cuda import SoftDTW + + +class AssemblyEnv(DirectRLEnv): + cfg: AssemblyEnvCfg + + def __init__(self, cfg: AssemblyEnvCfg, render_mode: str | None = None, **kwargs): + + # Update number of obs/states + cfg.observation_space = sum([OBS_DIM_CFG[obs] for obs in cfg.obs_order]) + cfg.state_space = sum([STATE_DIM_CFG[state] for state in cfg.state_order]) + self.cfg_task = cfg.tasks[cfg.task_name] + + super().__init__(cfg, render_mode, **kwargs) + + self._set_body_inertias() + self._init_tensors() + self._set_default_dynamics_parameters() + self._compute_intermediate_values(dt=self.physics_dt) + + # Load asset meshes in warp for SDF-based dense reward + wp.init() + self.wp_device = wp.get_preferred_device() + self.plug_mesh, self.plug_sample_points, self.socket_mesh = industreal_algo.load_asset_mesh_in_warp( + self.cfg_task.assembly_dir + self.cfg_task.held_asset_cfg.obj_path, + self.cfg_task.assembly_dir + self.cfg_task.fixed_asset_cfg.obj_path, + self.cfg_task.num_mesh_sample_points, + self.wp_device, + ) + + # Get the gripper open width based on plug object bounding box + self.gripper_open_width = automate_algo.get_gripper_open_width( + self.cfg_task.assembly_dir + self.cfg_task.held_asset_cfg.obj_path + ) + + # Create criterion for dynamic time warping (later used for imitation reward) + self.soft_dtw_criterion = SoftDTW(use_cuda=True, gamma=self.cfg_task.soft_dtw_gamma) + + # Evaluate + if self.cfg_task.if_logging_eval: + self._init_eval_logging() + + if self.cfg_task.sample_from != "rand": + self._init_eval_loading() + + wandb.init(project="automate", name=self.cfg_task.assembly_id + "_" + datetime.now().strftime("%m/%d/%Y")) + + def _init_eval_loading(self): + eval_held_asset_pose, eval_fixed_asset_pose, eval_success = automate_log.load_log_from_hdf5( + self.cfg_task.eval_filename + ) + + if self.cfg_task.sample_from == "gp": + self.gp = automate_algo.model_succ_w_gp(eval_held_asset_pose, eval_fixed_asset_pose, eval_success) + elif self.cfg_task.sample_from == "gmm": + self.gmm = automate_algo.model_succ_w_gmm(eval_held_asset_pose, eval_fixed_asset_pose, eval_success) + + def _init_eval_logging(self): + + self.held_asset_pose_log = torch.empty( + (0, 7), dtype=torch.float32, device=self.device + ) # (position, quaternion) + self.fixed_asset_pose_log = torch.empty((0, 7), dtype=torch.float32, device=self.device) + self.success_log = torch.empty((0, 1), dtype=torch.float32, device=self.device) + + # Turn off SBC during evaluation so all plugs are initialized outside of the socket + self.cfg_task.if_sbc = False + + def _set_body_inertias(self): + """Note: this is to account for the asset_options.armature parameter in IGE.""" + inertias = self._robot.root_physx_view.get_inertias() + offset = torch.zeros_like(inertias) + offset[:, :, [0, 4, 8]] += 0.01 + new_inertias = inertias + offset + self._robot.root_physx_view.set_inertias(new_inertias, torch.arange(self.num_envs)) + + def _set_default_dynamics_parameters(self): + """Set parameters defining dynamic interactions.""" + self.default_gains = torch.tensor(self.cfg.ctrl.default_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + + self.pos_threshold = torch.tensor(self.cfg.ctrl.pos_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + self.rot_threshold = torch.tensor(self.cfg.ctrl.rot_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + + # Set masses and frictions. + self._set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction) + self._set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction) + self._set_friction(self._robot, self.cfg_task.robot_cfg.friction) + + def _set_friction(self, asset, value): + """Update material properties for a given asset.""" + materials = asset.root_physx_view.get_material_properties() + materials[..., 0] = value # Static friction. + materials[..., 1] = value # Dynamic friction. + env_ids = torch.arange(self.scene.num_envs, device="cpu") + asset.root_physx_view.set_material_properties(materials, env_ids) + + def _init_tensors(self): + """Initialize tensors once.""" + self.identity_quat = ( + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) + + # Control targets. + self.ctrl_target_joint_pos = torch.zeros((self.num_envs, self._robot.num_joints), device=self.device) + self.ctrl_target_fingertip_midpoint_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.ctrl_target_fingertip_midpoint_quat = torch.zeros((self.num_envs, 4), device=self.device) + + # Fixed asset. + self.fixed_pos_action_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.fixed_pos_obs_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.init_fixed_pos_obs_noise = torch.zeros((self.num_envs, 3), device=self.device) + + # Held asset + held_base_x_offset = 0.0 + held_base_z_offset = 0.0 + + self.held_base_pos_local = torch.tensor([0.0, 0.0, 0.0], device=self.device).repeat((self.num_envs, 1)) + self.held_base_pos_local[:, 0] = held_base_x_offset + self.held_base_pos_local[:, 2] = held_base_z_offset + self.held_base_quat_local = self.identity_quat.clone().detach() + + self.held_base_pos = torch.zeros_like(self.held_base_pos_local) + self.held_base_quat = self.identity_quat.clone().detach() + + self.plug_grasps, self.disassembly_dists = self._load_assembly_info() + self.curriculum_height_bound, self.curriculum_height_step = self._get_curriculum_info(self.disassembly_dists) + self._load_disassembly_data() + + # Load grasp pose from json files given assembly ID + # Grasp pose tensors + self.palm_to_finger_center = ( + torch.tensor([0.0, 0.0, -self.cfg_task.palm_to_finger_dist], device=self.device) + .unsqueeze(0) + .repeat(self.num_envs, 1) + ) + self.robot_to_gripper_quat = ( + torch.tensor([0.0, 1.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) + self.plug_grasp_pos_local = self.plug_grasps[: self.num_envs, :3] + self.plug_grasp_quat_local = torch.roll(self.plug_grasps[: self.num_envs, 3:], -1, 1) + + # Computer body indices. + self.left_finger_body_idx = self._robot.body_names.index("panda_leftfinger") + self.right_finger_body_idx = self._robot.body_names.index("panda_rightfinger") + self.fingertip_body_idx = self._robot.body_names.index("panda_fingertip_centered") + + # Tensors for finite-differencing. + self.last_update_timestamp = 0.0 # Note: This is for finite differencing body velocities. + self.prev_fingertip_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.prev_fingertip_quat = self.identity_quat.clone() + self.prev_joint_pos = torch.zeros((self.num_envs, 7), device=self.device) + + # Keypoint tensors. + self.target_held_base_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.target_held_base_quat = self.identity_quat.clone().detach() + + offsets = self._get_keypoint_offsets(self.cfg_task.num_keypoints) + self.keypoint_offsets = offsets * self.cfg_task.keypoint_scale + self.keypoints_held = torch.zeros((self.num_envs, self.cfg_task.num_keypoints, 3), device=self.device) + self.keypoints_fixed = torch.zeros_like(self.keypoints_held, device=self.device) + + # Used to compute target poses. + self.fixed_success_pos_local = torch.zeros((self.num_envs, 3), device=self.device) + self.fixed_success_pos_local[:, 2] = 0.0 + + self.ep_succeeded = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + self.ep_success_times = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + + # SBC + if self.cfg_task.if_sbc: + self.curr_max_disp = self.curriculum_height_bound[:, 0] + else: + self.curr_max_disp = self.curriculum_height_bound[:, 1] + + def _load_assembly_info(self): + """Load grasp pose and disassembly distance for plugs in each environment.""" + + retrieve_file_path(self.cfg_task.plug_grasp_json, download_dir="./") + with open(os.path.basename(self.cfg_task.plug_grasp_json)) as f: + plug_grasp_dict = json.load(f) + plug_grasps = [plug_grasp_dict[f"asset_{self.cfg_task.assembly_id}"] for i in range(self.num_envs)] + + retrieve_file_path(self.cfg_task.disassembly_dist_json, download_dir="./") + with open(os.path.basename(self.cfg_task.disassembly_dist_json)) as f: + disassembly_dist_dict = json.load(f) + disassembly_dists = [disassembly_dist_dict[f"asset_{self.cfg_task.assembly_id}"] for i in range(self.num_envs)] + + return torch.as_tensor(plug_grasps).to(self.device), torch.as_tensor(disassembly_dists).to(self.device) + + def _get_curriculum_info(self, disassembly_dists): + """Calculate the ranges and step sizes for Sampling-based Curriculum (SBC) in each environment.""" + + curriculum_height_bound = torch.zeros((self.num_envs, 2), dtype=torch.float32, device=self.device) + curriculum_height_step = torch.zeros((self.num_envs, 2), dtype=torch.float32, device=self.device) + + curriculum_height_bound[:, 1] = disassembly_dists + self.cfg_task.curriculum_freespace_range + + curriculum_height_step[:, 0] = curriculum_height_bound[:, 1] / self.cfg_task.num_curriculum_step + curriculum_height_step[:, 1] = -curriculum_height_step[:, 0] / 2.0 + + return curriculum_height_bound, curriculum_height_step + + def _load_disassembly_data(self): + """Load pre-collected disassembly trajectories (end-effector position only).""" + + retrieve_file_path(self.cfg_task.disassembly_path_json, download_dir="./") + with open(os.path.basename(self.cfg_task.disassembly_path_json)) as f: + disassembly_traj = json.load(f) + + eef_pos_traj = [] + + for i in range(len(disassembly_traj)): + curr_ee_traj = np.asarray(disassembly_traj[i]["fingertip_centered_pos"]).reshape((-1, 3)) + curr_ee_goal = np.asarray(disassembly_traj[i]["fingertip_centered_pos"]).reshape((-1, 3))[0, :] + + # offset each trajectory to be relative to the goal + eef_pos_traj.append(curr_ee_traj - curr_ee_goal) + + self.eef_pos_traj = torch.tensor(eef_pos_traj, dtype=torch.float32, device=self.device).squeeze() + + def _get_keypoint_offsets(self, num_keypoints): + """Get uniformly-spaced keypoints along a line of unit length, centered at 0.""" + keypoint_offsets = torch.zeros((num_keypoints, 3), device=self.device) + keypoint_offsets[:, -1] = torch.linspace(0.0, 1.0, num_keypoints, device=self.device) - 0.5 + + return keypoint_offsets + + def _setup_scene(self): + """Initialize simulation scene.""" + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg(), translation=(0.0, 0.0, -0.4)) + + # spawn a usd file of a table into the scene + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") + cfg.func( + "/World/envs/env_.*/Table", cfg, translation=(0.55, 0.0, 0.0), orientation=(0.70711, 0.0, 0.0, 0.70711) + ) + + self._robot = Articulation(self.cfg.robot) + self._fixed_asset = Articulation(self.cfg_task.fixed_asset) + self._held_asset = RigidObject(self.cfg_task.held_asset) + + self.scene.clone_environments(copy_from_source=False) + self.scene.filter_collisions() + + self.scene.articulations["robot"] = self._robot + self.scene.articulations["fixed_asset"] = self._fixed_asset + self.scene.rigid_objects["held_asset"] = self._held_asset + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _compute_intermediate_values(self, dt): + """Get values computed from raw tensors. This includes adding noise.""" + # TODO: A lot of these can probably only be set once? + self.fixed_pos = self._fixed_asset.data.root_pos_w - self.scene.env_origins + self.fixed_quat = self._fixed_asset.data.root_quat_w + + self.held_pos = self._held_asset.data.root_pos_w - self.scene.env_origins + self.held_quat = self._held_asset.data.root_quat_w + + self.fingertip_midpoint_pos = self._robot.data.body_pos_w[:, self.fingertip_body_idx] - self.scene.env_origins + self.fingertip_midpoint_quat = self._robot.data.body_quat_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w[:, self.fingertip_body_idx] + + jacobians = self._robot.root_physx_view.get_jacobians() + + self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] + self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] + self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 + self.arm_mass_matrix = self._robot.root_physx_view.get_generalized_mass_matrices()[:, 0:7, 0:7] + self.joint_pos = self._robot.data.joint_pos.clone() + self.joint_vel = self._robot.data.joint_vel.clone() + + # Compute pose of gripper goal and top of socket in socket frame + self.gripper_goal_quat, self.gripper_goal_pos = torch_utils.tf_combine( + self.fixed_quat, + self.fixed_pos, + self.plug_grasp_quat_local, + self.plug_grasp_pos_local, + ) + + self.gripper_goal_quat, self.gripper_goal_pos = torch_utils.tf_combine( + self.gripper_goal_quat, + self.gripper_goal_pos, + self.robot_to_gripper_quat, + self.palm_to_finger_center, + ) + + # Finite-differencing results in more reliable velocity estimates. + self.ee_linvel_fd = (self.fingertip_midpoint_pos - self.prev_fingertip_pos) / dt + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + + # Add state differences if velocity isn't being added. + rot_diff_quat = torch_utils.quat_mul( + self.fingertip_midpoint_quat, torch_utils.quat_conjugate(self.prev_fingertip_quat) + ) + rot_diff_quat *= torch.sign(rot_diff_quat[:, 0]).unsqueeze(-1) + rot_diff_aa = axis_angle_from_quat(rot_diff_quat) + self.ee_angvel_fd = rot_diff_aa / dt + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + joint_diff = self.joint_pos[:, 0:7] - self.prev_joint_pos + self.joint_vel_fd = joint_diff / dt + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + + # Keypoint tensors. + self.held_base_quat[:], self.held_base_pos[:] = torch_utils.tf_combine( + self.held_quat, self.held_pos, self.held_base_quat_local, self.held_base_pos_local + ) + self.target_held_base_quat[:], self.target_held_base_pos[:] = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, self.fixed_success_pos_local + ) + + # Compute pos of keypoints on held asset, and fixed asset in world frame + for idx, keypoint_offset in enumerate(self.keypoint_offsets): + self.keypoints_held[:, idx] = torch_utils.tf_combine( + self.held_base_quat, self.held_base_pos, self.identity_quat, keypoint_offset.repeat(self.num_envs, 1) + )[1] + self.keypoints_fixed[:, idx] = torch_utils.tf_combine( + self.target_held_base_quat, + self.target_held_base_pos, + self.identity_quat, + keypoint_offset.repeat(self.num_envs, 1), + )[1] + + self.keypoint_dist = torch.norm(self.keypoints_held - self.keypoints_fixed, p=2, dim=-1).mean(-1) + self.last_update_timestamp = self._robot._data._sim_timestamp + + def _get_observations(self): + """Get actor/critic inputs using asymmetric critic.""" + + obs_dict = { + "joint_pos": self.joint_pos[:, 0:7], + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_quat": self.fingertip_midpoint_quat, + "fingertip_goal_pos": self.gripper_goal_pos, + "fingertip_goal_quat": self.gripper_goal_quat, + "delta_pos": self.gripper_goal_pos - self.fingertip_midpoint_pos, + } + + state_dict = { + "joint_pos": self.joint_pos[:, 0:7], + "joint_vel": self.joint_vel[:, 0:7], + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_quat": self.fingertip_midpoint_quat, + "ee_linvel": self.fingertip_midpoint_linvel, + "ee_angvel": self.fingertip_midpoint_angvel, + "fingertip_goal_pos": self.gripper_goal_pos, + "fingertip_goal_quat": self.gripper_goal_quat, + "held_pos": self.held_pos, + "held_quat": self.held_quat, + "delta_pos": self.gripper_goal_pos - self.fingertip_midpoint_pos, + } + # obs_tensors = [obs_dict[obs_name] for obs_name in self.cfg.obs_order + ['prev_actions']] + obs_tensors = [obs_dict[obs_name] for obs_name in self.cfg.obs_order] + obs_tensors = torch.cat(obs_tensors, dim=-1) + + # state_tensors = [state_dict[state_name] for state_name in self.cfg.state_order + ['prev_actions']] + state_tensors = [state_dict[state_name] for state_name in self.cfg.state_order] + state_tensors = torch.cat(state_tensors, dim=-1) + + return {"policy": obs_tensors, "critic": state_tensors} + + def _reset_buffers(self, env_ids): + """Reset buffers.""" + self.ep_succeeded[env_ids] = 0 + + def _pre_physics_step(self, action): + """Apply policy actions with smoothing.""" + env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(env_ids) > 0: + self._reset_buffers(env_ids) + + self.actions = ( + self.cfg.ctrl.ema_factor * action.clone().to(self.device) + (1 - self.cfg.ctrl.ema_factor) * self.actions + ) + + def move_gripper_in_place(self, ctrl_target_gripper_dof_pos): + """Keep gripper in current position as gripper closes.""" + actions = torch.zeros((self.num_envs, 6), device=self.device) + ctrl_target_gripper_dof_pos = 0.0 + + # Interpret actions as target pos displacements and set pos target + pos_actions = actions[:, 0:3] * self.pos_threshold + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = actions[:, 3:6] + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1.0e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = ctrl_target_gripper_dof_pos + self.generate_ctrl_signals() + + def _apply_action(self): + """Apply actions for policy as delta targets from current position.""" + # Get current yaw for success checking. + _, _, curr_yaw = torch_utils.get_euler_xyz(self.fingertip_midpoint_quat) + self.curr_yaw = torch.where(curr_yaw > np.deg2rad(235), curr_yaw - 2 * np.pi, curr_yaw) + + # Note: We use finite-differenced velocities for control and observations. + # Check if we need to re-compute velocities within the decimation loop. + if self.last_update_timestamp < self._robot._data._sim_timestamp: + self._compute_intermediate_values(dt=self.physics_dt) + + # Interpret actions as target pos displacements and set pos target + pos_actions = self.actions[:, 0:3] * self.pos_threshold + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = self.actions[:, 3:6] + if self.cfg_task.unidirectional_rot: + rot_actions[:, 2] = -(rot_actions[:, 2] + 1.0) * 0.5 # [-1, 0] + rot_actions = rot_actions * self.rot_threshold + + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + # To speed up learning, never allow the policy to move more than 5cm away from the base. + delta_pos = self.ctrl_target_fingertip_midpoint_pos - self.fixed_pos_action_frame + pos_error_clipped = torch.clip( + delta_pos, -self.cfg.ctrl.pos_action_bounds[0], self.cfg.ctrl.pos_action_bounds[1] + ) + self.ctrl_target_fingertip_midpoint_pos = self.fixed_pos_action_frame + pos_error_clipped + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 # Restrict actions to be upright. + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = 0.0 + self.generate_ctrl_signals() + + def _set_gains(self, prop_gains, rot_deriv_scale=1.0): + """Set robot gains using critical damping.""" + self.task_prop_gains = prop_gains + self.task_deriv_gains = 2 * torch.sqrt(prop_gains) + self.task_deriv_gains[:, 3:6] /= rot_deriv_scale + + def generate_ctrl_signals(self): + """Get Jacobian. Set Franka DOF position targets (fingers) or DOF torques (arm).""" + self.joint_torque, self.applied_wrench = fc.compute_dof_torque( + cfg=self.cfg, + dof_pos=self.joint_pos, + dof_vel=self.joint_vel, # _fd, + fingertip_midpoint_pos=self.fingertip_midpoint_pos, + fingertip_midpoint_quat=self.fingertip_midpoint_quat, + fingertip_midpoint_linvel=self.ee_linvel_fd, + fingertip_midpoint_angvel=self.ee_angvel_fd, + jacobian=self.fingertip_midpoint_jacobian, + arm_mass_matrix=self.arm_mass_matrix, + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat, + task_prop_gains=self.task_prop_gains, + task_deriv_gains=self.task_deriv_gains, + device=self.device, + ) + + # set target for gripper joints to use GYM's PD controller + self.ctrl_target_joint_pos[:, 7:9] = self.ctrl_target_gripper_dof_pos + self.joint_torque[:, 7:9] = 0.0 + + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + self._robot.set_joint_effort_target(self.joint_torque) + + def _get_dones(self): + """Update intermediate values used for rewards and observations.""" + self._compute_intermediate_values(dt=self.physics_dt) + time_out = self.episode_length_buf >= self.max_episode_length - 1 + return time_out, time_out + + def _get_rewards(self): + """Update rewards and compute success statistics.""" + # Get successful and failed envs at current timestep + + curr_successes = automate_algo.check_plug_inserted_in_socket( + self.held_pos, + self.fixed_pos, + self.disassembly_dists, + self.keypoints_held, + self.keypoints_fixed, + self.cfg_task.close_error_thresh, + self.episode_length_buf, + ) + + rew_buf = self._update_rew_buf(curr_successes) + self.ep_succeeded = torch.logical_or(self.ep_succeeded, curr_successes) + + wandb.log(self.extras) + + # Only log episode success rates at the end of an episode. + if torch.any(self.reset_buf): + self.extras["successes"] = torch.count_nonzero(self.ep_succeeded) / self.num_envs + + sbc_rwd_scale = automate_algo.get_curriculum_reward_scale( + curr_max_disp=self.curr_max_disp, + curriculum_height_bound=self.curriculum_height_bound, + ) + + rew_buf *= sbc_rwd_scale + + if self.cfg_task.if_sbc: + + self.curr_max_disp = automate_algo.get_new_max_disp( + curr_success=torch.count_nonzero(self.ep_succeeded) / self.num_envs, + cfg_task=self.cfg_task, + curriculum_height_bound=self.curriculum_height_bound, + curriculum_height_step=self.curriculum_height_step, + curr_max_disp=self.curr_max_disp, + ) + + self.extras["curr_max_disp"] = self.curr_max_disp + wandb.log({ + "success": torch.mean(self.ep_succeeded.float()), + "reward": torch.mean(rew_buf), + "sbc_rwd_scale": sbc_rwd_scale, + }) + + if self.cfg_task.if_logging_eval: + self.success_log = torch.cat([self.success_log, self.ep_succeeded.reshape((self.num_envs, 1))], dim=0) + + if self.success_log.shape[0] >= self.cfg_task.num_eval_trials: + automate_log.write_log_to_hdf5( + self.held_asset_pose_log, + self.fixed_asset_pose_log, + self.success_log, + self.cfg_task.eval_filename, + ) + exit(0) + + self.prev_actions = self.actions.clone() + return rew_buf + + def _update_rew_buf(self, curr_successes): + """Compute reward at current timestep.""" + rew_dict = dict({}) + + # SDF-based reward. + rew_dict["sdf"] = industreal_algo.get_sdf_reward( + self.plug_mesh, + self.plug_sample_points, + self.held_pos, + self.held_quat, + self.fixed_pos, + self.fixed_quat, + self.wp_device, + self.device, + ) + + rew_dict["curr_successes"] = curr_successes.clone().float() + + # Imitation Reward: Calculate reward + curr_eef_pos = (self.fingertip_midpoint_pos - self.gripper_goal_pos).reshape( + -1, 3 + ) # relative position instead of absolute position + rew_dict["imitation"] = automate_algo.get_imitation_reward_from_dtw( + self.eef_pos_traj, curr_eef_pos, self.prev_fingertip_midpoint_pos, self.soft_dtw_criterion, self.device + ) + + self.prev_fingertip_midpoint_pos = torch.cat( + (self.prev_fingertip_midpoint_pos[:, 1:, :], curr_eef_pos.unsqueeze(1).clone().detach()), dim=1 + ) + + rew_buf = ( + self.cfg_task.sdf_rwd_scale * rew_dict["sdf"] + + self.cfg_task.imitation_rwd_scale * rew_dict["imitation"] + + rew_dict["curr_successes"] + ) + + for rew_name, rew in rew_dict.items(): + self.extras[f"logs_rew_{rew_name}"] = rew.mean() + + return rew_buf + + def _reset_idx(self, env_ids): + """ + We assume all envs will always be reset at the same time. + """ + super()._reset_idx(env_ids) + + self._set_assets_to_default_pose(env_ids) + self._set_franka_to_default_pose(joints=self.cfg.ctrl.reset_joints, env_ids=env_ids) + self.step_sim_no_action() + + self.randomize_initial_state(env_ids) + + if self.cfg_task.if_logging_eval: + self.held_asset_pose_log = torch.cat( + [self.held_asset_pose_log, torch.cat([self.held_pos, self.held_quat], dim=1)], dim=0 + ) + self.fixed_asset_pose_log = torch.cat( + [self.fixed_asset_pose_log, torch.cat([self.fixed_pos, self.fixed_quat], dim=1)], dim=0 + ) + + prev_fingertip_midpoint_pos = (self.fingertip_midpoint_pos - self.gripper_goal_pos).unsqueeze( + 1 + ) # (num_envs, 1, 3) + self.prev_fingertip_midpoint_pos = torch.repeat_interleave( + prev_fingertip_midpoint_pos, self.cfg_task.num_point_robot_traj, dim=1 + ) # (num_envs, num_point_robot_traj, 3) + + def _set_assets_to_default_pose(self, env_ids): + """Move assets to default pose before randomization.""" + held_state = self._held_asset.data.default_root_state.clone()[env_ids] + held_state[:, 0:3] += self.scene.env_origins[env_ids] + held_state[:, 7:] = 0.0 + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7], env_ids=env_ids) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:], env_ids=env_ids) + self._held_asset.reset() + + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + fixed_state[:, 0:3] += self.scene.env_origins[env_ids] + fixed_state[:, 7:] = 0.0 + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.reset() + + def _move_gripper_to_grasp_pose(self, env_ids): + """Define grasp pose for plug and move gripper to pose.""" + + gripper_goal_quat, gripper_goal_pos = torch_utils.tf_combine( + self.held_quat, + self.held_pos, + self.plug_grasp_quat_local, + self.plug_grasp_pos_local, + ) + + gripper_goal_quat, gripper_goal_pos = torch_utils.tf_combine( + gripper_goal_quat, + gripper_goal_pos, + self.robot_to_gripper_quat, + self.palm_to_finger_center, + ) + + # Set target_pos + self.ctrl_target_fingertip_midpoint_pos = gripper_goal_pos.clone() + + # Set target rot + self.ctrl_target_fingertip_midpoint_quat = gripper_goal_quat.clone() + + self.set_pos_inverse_kinematics(env_ids) + self.step_sim_no_action() + + def set_pos_inverse_kinematics(self, env_ids): + """Set robot joint position using DLS IK.""" + ik_time = 0.0 + while ik_time < 0.50: + # Compute error to target. + pos_error, axis_angle_error = fc.get_pose_error( + fingertip_midpoint_pos=self.fingertip_midpoint_pos[env_ids], + fingertip_midpoint_quat=self.fingertip_midpoint_quat[env_ids], + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos[env_ids], + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat[env_ids], + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + + delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) + + # Solve DLS problem. + delta_dof_pos = fc._get_delta_dof_pos( + delta_pose=delta_hand_pose, + ik_method="dls", + jacobian=self.fingertip_midpoint_jacobian[env_ids], + device=self.device, + ) + self.joint_pos[env_ids, 0:7] += delta_dof_pos[:, 0:7] + self.joint_vel[env_ids, :] = torch.zeros_like(self.joint_pos[env_ids,]) + + self.ctrl_target_joint_pos[env_ids, 0:7] = self.joint_pos[env_ids, 0:7] + # Update dof state. + self._robot.write_joint_state_to_sim(self.joint_pos, self.joint_vel) + self._robot.reset() + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + + # Simulate and update tensors. + self.step_sim_no_action() + ik_time += self.physics_dt + + return pos_error, axis_angle_error + + def _set_franka_to_default_pose(self, joints, env_ids): + """Return Franka to its default joint position.""" + gripper_width = self.gripper_open_width + joint_pos = self._robot.data.default_joint_pos[env_ids] + joint_pos[:, 7:] = gripper_width # MIMIC + joint_pos[:, :7] = torch.tensor(joints, device=self.device)[None, :] + joint_vel = torch.zeros_like(joint_pos) + joint_effort = torch.zeros_like(joint_pos) + self.ctrl_target_joint_pos[env_ids, :] = joint_pos + self._robot.set_joint_position_target(self.ctrl_target_joint_pos[env_ids], env_ids=env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + self._robot.reset() + self._robot.set_joint_effort_target(joint_effort, env_ids=env_ids) + + self.step_sim_no_action() + + def step_sim_no_action(self): + """Step the simulation without an action. Used for resets.""" + self.scene.write_data_to_sim() + self.sim.step(render=True) + self.scene.update(dt=self.physics_dt) + self._compute_intermediate_values(dt=self.physics_dt) + + def randomize_fixed_initial_state(self, env_ids): + + # (1.) Randomize fixed asset pose. + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + # (1.a.) Position + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + fixed_asset_init_pos_rand = torch.tensor( + self.cfg_task.fixed_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + fixed_pos_init_rand = fixed_pos_init_rand @ torch.diag(fixed_asset_init_pos_rand) + fixed_state[:, 0:3] += fixed_pos_init_rand + self.scene.env_origins[env_ids] + fixed_state[:, 2] += self.cfg_task.fixed_asset_z_offset + + # (1.b.) Orientation + fixed_orn_init_yaw = np.deg2rad(self.cfg_task.fixed_asset_init_orn_deg) + fixed_orn_yaw_range = np.deg2rad(self.cfg_task.fixed_asset_init_orn_range_deg) + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_orn_euler = fixed_orn_init_yaw + fixed_orn_yaw_range * rand_sample + fixed_orn_euler[:, 0:2] = 0.0 # Only change yaw. + fixed_orn_quat = torch_utils.quat_from_euler_xyz( + fixed_orn_euler[:, 0], fixed_orn_euler[:, 1], fixed_orn_euler[:, 2] + ) + fixed_state[:, 3:7] = fixed_orn_quat + # (1.c.) Velocity + fixed_state[:, 7:] = 0.0 # vel + # (1.d.) Update values. + self._fixed_asset.write_root_state_to_sim(fixed_state, env_ids=env_ids) + self._fixed_asset.reset() + + # (1.e.) Noisy position observation. + fixed_asset_pos_noise = torch.randn((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_asset_pos_rand = torch.tensor(self.cfg.obs_rand.fixed_asset_pos, dtype=torch.float32, device=self.device) + fixed_asset_pos_noise = fixed_asset_pos_noise @ torch.diag(fixed_asset_pos_rand) + self.init_fixed_pos_obs_noise[:] = fixed_asset_pos_noise + + self.step_sim_no_action() + + def randomize_held_initial_state(self, env_ids, pre_grasp): + + curr_curriculum_disp_range = self.curriculum_height_bound[:, 1] - self.curr_max_disp + if pre_grasp: + self.curriculum_disp = self.curr_max_disp + curr_curriculum_disp_range * ( + torch.rand((self.num_envs,), dtype=torch.float32, device=self.device) + ) + + if self.cfg_task.sample_from == "rand": + + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + held_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + held_asset_init_pos_rand = torch.tensor( + self.cfg_task.held_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + self.held_pos_init_rand = held_pos_init_rand @ torch.diag(held_asset_init_pos_rand) + + if self.cfg_task.sample_from == "gp": + rand_sample = torch.rand((self.cfg_task.num_gp_candidates, 3), dtype=torch.float32, device=self.device) + held_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + held_asset_init_pos_rand = torch.tensor( + self.cfg_task.held_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + held_asset_init_candidates = held_pos_init_rand @ torch.diag(held_asset_init_pos_rand) + self.held_pos_init_rand, _ = automate_algo.propose_failure_samples_batch_from_gp( + self.gp, held_asset_init_candidates.cpu().detach().numpy(), len(env_ids), self.device + ) + + if self.cfg_task.sample_from == "gmm": + self.held_pos_init_rand = automate_algo.sample_rel_pos_from_gmm(self.gmm, len(env_ids), self.device) + + # Set plug pos to assembled state, but offset plug Z-coordinate by height of socket, + # minus curriculum displacement + held_state = self._held_asset.data.default_root_state.clone() + held_state[env_ids, 0:3] = self.fixed_pos[env_ids].clone() + self.scene.env_origins[env_ids] + held_state[env_ids, 3:7] = self.fixed_quat[env_ids].clone() + held_state[env_ids, 7:] = 0.0 + + held_state[env_ids, 2] += self.curriculum_disp + + plug_in_freespace_idx = torch.argwhere(self.curriculum_disp > self.disassembly_dists) + held_state[plug_in_freespace_idx, :2] += self.held_pos_init_rand[plug_in_freespace_idx, :2] + + self._held_asset.write_root_state_to_sim(held_state) + self._held_asset.reset() + + self.step_sim_no_action() + + def randomize_initial_state(self, env_ids): + """Randomize initial state and perform any episode-level randomization.""" + # Disable gravity. + physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) + + self.randomize_fixed_initial_state(env_ids) + + # Compute the frame on the bolt that would be used as observation: fixed_pos_obs_frame + # For example, the tip of the bolt can be used as the observation frame + fixed_tip_pos_local = torch.zeros_like(self.fixed_pos) + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.height + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.base_height + + _, fixed_tip_pos = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, fixed_tip_pos_local + ) + self.fixed_pos_obs_frame[:] = fixed_tip_pos + + self.randomize_held_initial_state(env_ids, pre_grasp=True) + + self._move_gripper_to_grasp_pose(env_ids) + + self.randomize_held_initial_state(env_ids, pre_grasp=False) + + # Close hand + # Set gains to use for quick resets. + reset_task_prop_gains = torch.tensor(self.cfg.ctrl.reset_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + reset_rot_deriv_scale = self.cfg.ctrl.reset_rot_deriv_scale + self._set_gains(reset_task_prop_gains, reset_rot_deriv_scale) + + self.step_sim_no_action() + + grasp_time = 0.0 + while grasp_time < 0.25: + self.ctrl_target_joint_pos[env_ids, 7:] = 0.0 # Close gripper. + self.ctrl_target_gripper_dof_pos = 0.0 + self.move_gripper_in_place(ctrl_target_gripper_dof_pos=0.0) + self.step_sim_no_action() + grasp_time += self.sim.get_physics_dt() + + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + # Set initial actions to involve no-movement. Needed for EMA/correct penalties. + self.actions = torch.zeros_like(self.actions) + self.prev_actions = torch.zeros_like(self.actions) + self.fixed_pos_action_frame[:] = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + + # Zero initial velocity. + self.ee_angvel_fd[:, :] = 0.0 + self.ee_linvel_fd[:, :] = 0.0 + + # Set initial gains for the episode. + self._set_gains(self.default_gains) + + physics_sim_view.set_gravity(carb.Float3(*self.cfg.sim.gravity)) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env_cfg.py new file mode 100644 index 00000000000..ce8d2004aa8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_env_cfg.py @@ -0,0 +1,205 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass + +from .assembly_tasks_cfg import ASSET_DIR, Insertion + +OBS_DIM_CFG = { + "joint_pos": 7, + "fingertip_pos": 3, + "fingertip_quat": 4, + "fingertip_goal_pos": 3, + "fingertip_goal_quat": 4, + "delta_pos": 3, +} + +STATE_DIM_CFG = { + "joint_pos": 7, + "joint_vel": 7, + "fingertip_pos": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, + "fingertip_goal_pos": 3, + "fingertip_goal_quat": 4, + "held_pos": 3, + "held_quat": 4, + "delta_pos": 3, +} + + +@configclass +class ObsRandCfg: + fixed_asset_pos = [0.001, 0.001, 0.001] + + +@configclass +class CtrlCfg: + ema_factor = 0.2 + + pos_action_bounds = [0.1, 0.1, 0.1] + rot_action_bounds = [0.01, 0.01, 0.01] + + pos_action_threshold = [0.1, 0.1, 0.1] + rot_action_threshold = [0.01, 0.01, 0.01] + + reset_joints = [0.0, 0.0, 0.0, -1.870, 0.0, 1.8675, 0.785398] + reset_task_prop_gains = [1000, 1000, 1000, 50, 50, 50] + # reset_rot_deriv_scale = 1.0 + # default_task_prop_gains = [1000, 1000, 1000, 50, 50, 50] + # reset_task_prop_gains = [300, 300, 300, 20, 20, 20] + reset_rot_deriv_scale = 10.0 + default_task_prop_gains = [100, 100, 100, 30, 30, 30] + + # Null space parameters. + default_dof_pos_tensor = [0.0, 0.0, 0.0, -1.870, 0.0, 1.8675, 0.785398] + kp_null = 10.0 + kd_null = 6.3246 + + +@configclass +class AssemblyEnvCfg(DirectRLEnvCfg): + decimation = 8 + action_space = 6 + # num_*: will be overwritten to correspond to obs_order, state_order. + observation_space = 24 + state_space = 44 + obs_order: list = [ + "joint_pos", + "fingertip_pos", + "fingertip_quat", + "fingertip_goal_pos", + "fingertip_goal_quat", + "delta_pos", + ] + state_order: list = [ + "joint_pos", + "joint_vel", + "fingertip_pos", + "fingertip_quat", + "ee_linvel", + "ee_angvel", + "fingertip_goal_pos", + "fingertip_goal_quat", + "held_pos", + "held_quat", + "delta_pos", + ] + + task_name: str = "insertion" # peg_insertion, gear_meshing, nut_threading + tasks: dict = {"insertion": Insertion()} + obs_rand: ObsRandCfg = ObsRandCfg() + ctrl: CtrlCfg = CtrlCfg() + + # episode_length_s = 10.0 # Probably need to override. + episode_length_s = 5.0 + sim: SimulationCfg = SimulationCfg( + device="cuda:0", + dt=1 / 120, + gravity=(0.0, 0.0, -9.81), + physx=PhysxCfg( + solver_type=1, + max_position_iteration_count=192, # Important to avoid interpenetration. + max_velocity_iteration_count=1, + bounce_threshold_velocity=0.2, + friction_offset_threshold=0.01, + friction_correlation_distance=0.00625, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + gpu_max_num_partitions=1, # Important for stable simulation. + ), + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + ) + + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0) + + robot = ArticulationCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ASSET_DIR}/franka_mimic.usd", + # usd_path=f'{ASSET_DIR}/automate_franka.usd', + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.00871, + "panda_joint2": -0.10368, + "panda_joint3": -0.00794, + "panda_joint4": -1.49139, + "panda_joint5": -0.00083, + "panda_joint6": 1.38774, + "panda_joint7": 0.0, + "panda_finger_joint2": 0.04, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + actuators={ + "panda_arm1": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=87, + velocity_limit=124.6, + ), + "panda_arm2": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=12, + velocity_limit=149.5, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint[1-2]"], + effort_limit=40.0, + velocity_limit=0.04, + stiffness=7500.0, + damping=173.0, + friction=0.1, + armature=0.0, + ), + }, + ) + # contact_sensor: ContactSensorCfg = ContactSensorCfg( + # prim_path="/World/envs/env_.*/Robot/.*", update_period=0.0, history_length=1, debug_vis=True + # ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py new file mode 100644 index 00000000000..5ecacfb8f9d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py @@ -0,0 +1,279 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +ASSET_DIR = f"{ISAACLAB_NUCLEUS_DIR}/AutoMate" + +OBS_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, +} + +STATE_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, + "joint_pos": 7, + "held_pos": 3, + "held_pos_rel_fixed": 3, + "held_quat": 4, + "fixed_pos": 3, + "fixed_quat": 4, + "task_prop_gains": 6, + "ema_factor": 1, + "pos_threshold": 3, + "rot_threshold": 3, +} + + +@configclass +class FixedAssetCfg: + usd_path: str = "" + diameter: float = 0.0 + height: float = 0.0 + base_height: float = 0.0 # Used to compute held asset CoM. + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class HeldAssetCfg: + usd_path: str = "" + diameter: float = 0.0 # Used for gripper width. + height: float = 0.0 + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class RobotCfg: + robot_usd: str = "" + franka_fingerpad_length: float = 0.017608 + friction: float = 0.75 + + +@configclass +class AssemblyTask: + robot_cfg: RobotCfg = RobotCfg() + name: str = "" + duration_s = 5.0 + + fixed_asset_cfg: FixedAssetCfg = FixedAssetCfg() + held_asset_cfg: HeldAssetCfg = HeldAssetCfg() + asset_size: float = 0.0 + + # palm_to_finger_dist: float = 0.1034 + palm_to_finger_dist: float = 0.1134 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.015] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0, 2.356] + hand_init_orn_noise: list = [0.0, 0.0, 1.57] + + # Action + unidirectional_rot: bool = False + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 10.0 + + # Held Asset (applies to all tasks) + # held_asset_pos_noise: list = [0.0, 0.006, 0.003] # noise level of the held asset in gripper + held_asset_init_pos_noise: list = [0.01, 0.01, 0.01] + held_asset_pos_noise: list = [0.0, 0.0, 0.0] + held_asset_rot_init: float = 0.0 + + # Reward + ee_success_yaw: float = 0.0 # nut_threading task only. + action_penalty_scale: float = 0.0 + action_grad_penalty_scale: float = 0.0 + # Reward function details can be found in Appendix B of https://arxiv.org/pdf/2408.04587. + # Multi-scale keypoints are used to capture different phases of the task. + # Each reward passes the keypoint distance, x, through a squashing function: + # r(x) = 1/(exp(-ax) + b + exp(ax)). + # Each list defines [a, b] which control the slope and maximum of the squashing function. + num_keypoints: int = 4 + keypoint_scale: float = 0.15 + + # Fixed-asset height fraction for which different bonuses are rewarded (see individual tasks). + success_threshold: float = 0.04 + engage_threshold: float = 0.9 + + # SDF reward + sdf_rwd_scale: float = 1.0 + num_mesh_sample_points: int = 1000 + + # Imitation reward + imitation_rwd_scale: float = 1.0 + soft_dtw_gamma: float = 0.01 # set to 0 if want to use the original DTW without any smoothing + num_point_robot_traj: int = 10 # number of waypoints included in the end-effector trajectory + + # SBC + initial_max_disp: float = 0.01 # max initial downward displacement of plug at beginning of curriculum + curriculum_success_thresh: float = 0.8 # success rate threshold for increasing curriculum difficulty + curriculum_failure_thresh: float = 0.5 # success rate threshold for decreasing curriculum difficulty + curriculum_freespace_range: float = 0.01 + num_curriculum_step: int = 10 + curriculum_height_step: list = [ + -0.005, + 0.003, + ] # how much to increase max initial downward displacement after hitting success or failure thresh + + if_sbc: bool = True + + # Logging evaluation results + if_logging_eval: bool = False + num_eval_trials: int = 100 + eval_filename: str = "evaluation_00015.h5" + + # Fine-tuning + sample_from: str = "rand" # gp, gmm, idv, rand + num_gp_candidates: int = 1000 + + +@configclass +class Peg8mm(HeldAssetCfg): + usd_path = "plug.usd" + obj_path = "plug.obj" + diameter = 0.007986 + height = 0.050 + mass = 0.019 + + +@configclass +class Hole8mm(FixedAssetCfg): + usd_path = "socket.usd" + obj_path = "socket.obj" + diameter = 0.0081 + height = 0.050896 + base_height = 0.0 + + +@configclass +class Insertion(AssemblyTask): + name = "insertion" + + assembly_id = "00015" + assembly_dir = f"{ASSET_DIR}/{assembly_id}/" + + fixed_asset_cfg = Hole8mm() + held_asset_cfg = Peg8mm() + asset_size = 8.0 + duration_s = 10.0 + + plug_grasp_json = f"{ASSET_DIR}/plug_grasps.json" + disassembly_dist_json = f"{ASSET_DIR}/disassembly_dist.json" + disassembly_path_json = f"{assembly_dir}/disassemble_traj.json" + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.047] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0.0, 0.0] + hand_init_orn_noise: list = [0.0, 0.0, 0.785] + hand_width_max: float = 0.080 # maximum opening width of gripper + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 10.0 + fixed_asset_z_offset: float = 0.1435 + + # Held Asset (applies to all tasks) + # held_asset_pos_noise: list = [0.003, 0.0, 0.003] # noise level of the held asset in gripper + held_asset_init_pos_noise: list = [0.01, 0.01, 0.01] + held_asset_pos_noise: list = [0.0, 0.0, 0.0] + held_asset_rot_init: float = 0.0 + + # Rewards + keypoint_coef_baseline: list = [5, 4] + keypoint_coef_coarse: list = [50, 2] + keypoint_coef_fine: list = [100, 0] + # Fraction of socket height. + success_threshold: float = 0.04 + engage_threshold: float = 0.9 + engage_height_thresh: float = 0.01 + success_height_thresh: float = 0.003 + close_error_thresh: float = 0.015 + + fixed_asset: ArticulationCfg = ArticulationCfg( + # fixed_asset: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/FixedAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{assembly_dir}{fixed_asset_cfg.usd_path}", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + fix_root_link=True, # add this so the fixed asset is set to have a fixed base + ), + mass_props=sim_utils.MassPropertiesCfg(mass=fixed_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + # init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.6, 0.0, 0.05), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={}, + joint_vel={}, + ), + actuators={}, + ) + # held_asset: ArticulationCfg = ArticulationCfg( + held_asset: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/HeldAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{assembly_dir}{held_asset_cfg.usd_path}", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=held_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + # init_state=ArticulationCfg.InitialStateCfg( + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), + rot=(1.0, 0.0, 0.0, 0.0), + # joint_pos={}, + # joint_vel={} + ), + # actuators={} + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_algo_utils.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_algo_utils.py new file mode 100644 index 00000000000..ebff98947ee --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_algo_utils.py @@ -0,0 +1,508 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import os +import sys +import torch +import trimesh + +import warp as wp + +print("Python Executable:", sys.executable) +print("Python Path:", sys.path) + +from scipy.stats import norm + +from sklearn.gaussian_process import GaussianProcessRegressor +from sklearn.mixture import GaussianMixture + +base_dir = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), ".")) +sys.path.append(base_dir) + +from isaaclab.utils.assets import retrieve_file_path + +""" +Initialization / Sampling +""" + + +def get_prev_success_init(held_asset_pose, fixed_asset_pose, success, N, device): + """ + Randomly selects N held_asset_pose and corresponding fixed_asset_pose + at indices where success is 1 and returns them as torch tensors. + + Args: + held_asset_pose (np.ndarray): Numpy array of held asset poses. + fixed_asset_pose (np.ndarray): Numpy array of fixed asset poses. + success (np.ndarray): Numpy array of success values (1 for success, 0 for failure). + N (int): Number of successful indices to select. + device: torch device. + + Returns: + tuple: (held_asset_poses, fixed_asset_poses) as torch tensors, or None if no success found. + """ + # Get indices where success is 1 + success_indices = np.where(success == 1)[0] + + if success_indices.size == 0: + return None # No successful entries found + + # Select up to N random indices from successful indices + selected_indices = np.random.choice(success_indices, min(N, len(success_indices)), replace=False) + + return torch.tensor(held_asset_pose[selected_indices], device=device), torch.tensor( + fixed_asset_pose[selected_indices], device=device + ) + + +def model_succ_w_gmm(held_asset_pose, fixed_asset_pose, success): + """ + Models the success rate distribution as a function of the relative position between the held and fixed assets + using a Gaussian Mixture Model (GMM). + + Parameters: + held_asset_pose (np.ndarray): Array of shape (N, 7) representing the positions of the held asset. + fixed_asset_pose (np.ndarray): Array of shape (N, 7) representing the positions of the fixed asset. + success (np.ndarray): Array of shape (N, 1) representing the success. + + Returns: + GaussianMixture: The fitted GMM. + + Example: + gmm = model_succ_dist_w_gmm(held_asset_pose, fixed_asset_pose, success) + relative_pose = held_asset_pose - fixed_asset_pose + # To compute the probability of each component for the given relative positions: + probabilities = gmm.predict_proba(relative_pose) + """ + # Compute the relative positions (held asset relative to fixed asset) + relative_pos = held_asset_pose[:, :3] - fixed_asset_pose[:, :3] + + # Flatten the success array to serve as sample weights. + # This way, samples with higher success contribute more to the model. + sample_weights = success.flatten() + + # Initialize the Gaussian Mixture Model with the specified number of components. + gmm = GaussianMixture(n_components=2, random_state=0) + + # Fit the GMM on the relative positions, using sample weights from the success metric. + gmm.fit(relative_pos, sample_weight=sample_weights) + + return gmm + + +def sample_rel_pos_from_gmm(gmm, batch_size, device): + """ + Samples a batch of relative poses (held_asset relative to fixed_asset) + from a fitted GaussianMixture model. + + Parameters: + gmm (GaussianMixture): A GaussianMixture model fitted on relative pose data. + batch_size (int): The number of samples to generate. + + Returns: + torch.Tensor: A tensor of shape (batch_size, 3) containing the sampled relative poses. + """ + # Sample batch_size samples from the Gaussian Mixture Model. + samples, _ = gmm.sample(batch_size) + + # Convert the numpy array to a torch tensor. + samples_tensor = torch.from_numpy(samples).to(device) + + return samples_tensor + + +def model_succ_w_gp(held_asset_pose, fixed_asset_pose, success): + """ + Models the success rate distribution given the relative position of the held asset + from the fixed asset using a Gaussian Process classifier. + + Parameters: + held_asset_pose (np.ndarray): Array of shape (N, 7) representing the held asset pose. + Assumes the first 3 columns are the (x, y, z) positions. + fixed_asset_pose (np.ndarray): Array of shape (N, 7) representing the fixed asset pose. + Assumes the first 3 columns are the (x, y, z) positions. + success (np.ndarray): Array of shape (N, 1) representing the success outcome (e.g., 0 for failure, + 1 for success). + + Returns: + GaussianProcessClassifier: A trained GP classifier that models the success rate. + """ + # Compute the relative position (using only the translation components) + relative_position = held_asset_pose[:, :3] - fixed_asset_pose[:, :3] + + # Flatten success array from (N, 1) to (N,) + y = success.ravel() + + # Create and fit the Gaussian Process Classifier + # gp = GaussianProcessClassifier(kernel=kernel, random_state=42) + gp = GaussianProcessRegressor() + gp.fit(relative_position, y) + + return gp + + +def propose_failure_samples_batch_from_gp( + gp_model, candidate_points, batch_size, device, method="ucb", kappa=2.0, xi=0.01 +): + """ + Proposes a batch of candidate samples from failure-prone regions using one of three acquisition functions: + 'ucb' (Upper Confidence Bound), 'pi' (Probability of Improvement), or 'ei' (Expected Improvement). + + In this formulation, lower predicted success probability (closer to 0) is desired, + so we invert the typical acquisition formulations. + + Parameters: + gp_model: A trained Gaussian Process model (e.g., GaussianProcessRegressor) that supports + predictions with uncertainties via the 'predict' method (with return_std=True). + candidate_points (np.ndarray): Array of shape (n_candidates, d) representing candidate relative positions. + batch_size (int): Number of candidate samples to propose. + method (str): Acquisition function to use: 'ucb', 'pi', or 'ei'. Default is 'ucb'. + kappa (float): Exploration parameter for UCB. Default is 2.0. + xi (float): Exploration parameter for PI and EI. Default is 0.01. + + Returns: + best_candidates (np.ndarray): Array of shape (batch_size, d) containing the selected candidate points. + acquisition (np.ndarray): Acquisition values computed for each candidate point. + """ + # Obtain the predictive mean and standard deviation for each candidate point. + mu, sigma = gp_model.predict(candidate_points, return_std=True) + # mu, sigma = gp_model.predict(candidate_points) + + # Compute the acquisition values based on the chosen method. + if method.lower() == "ucb": + # Inversion: we want low success (i.e. low mu) and high uncertainty (sigma) to be attractive. + acquisition = kappa * sigma - mu + elif method.lower() == "pi": + # Probability of Improvement: likelihood of the prediction falling below the target=0.0. + Z = (-mu - xi) / (sigma + 1e-9) + acquisition = norm.cdf(Z) + elif method.lower() == "ei": + # Expected Improvement + Z = (-mu - xi) / (sigma + 1e-9) + acquisition = (-mu - xi) * norm.cdf(Z) + sigma * norm.pdf(Z) + # Set acquisition to 0 where sigma is nearly zero. + acquisition[sigma < 1e-9] = 0.0 + else: + raise ValueError("Unknown acquisition method. Please choose 'ucb', 'pi', or 'ei'.") + + # Select the indices of the top batch_size candidates (highest acquisition values). + sorted_indices = np.argsort(acquisition)[::-1] # sort in descending order + best_indices = sorted_indices[:batch_size] + best_candidates = candidate_points[best_indices] + + # Convert the numpy array to a torch tensor. + best_candidates_tensor = torch.from_numpy(best_candidates).to(device) + + return best_candidates_tensor, acquisition + + +def propose_success_samples_batch_from_gp( + gp_model, candidate_points, batch_size, device, method="ucb", kappa=2.0, xi=0.01 +): + """ + Proposes a batch of candidate samples from high success rate regions using one of three acquisition functions: + 'ucb' (Upper Confidence Bound), 'pi' (Probability of Improvement), or 'ei' (Expected Improvement). + + In this formulation, higher predicted success probability is desired. + The GP model is assumed to provide predictions with uncertainties via its 'predict' method (using return_std=True). + + Parameters: + gp_model: A trained Gaussian Process model (e.g., GaussianProcessRegressor) that supports + predictions with uncertainties. + candidate_points (np.ndarray): Array of shape (n_candidates, d) representing candidate relative positions. + batch_size (int): Number of candidate samples to propose. + method (str): Acquisition function to use: 'ucb', 'pi', or 'ei'. Default is 'ucb'. + kappa (float): Exploration parameter for UCB. Default is 2.0. + xi (float): Exploration parameter for PI and EI. Default is 0.01. + + Returns: + best_candidates (np.ndarray): Array of shape (batch_size, d) containing the selected candidate points. + acquisition (np.ndarray): Acquisition values computed for each candidate point. + """ + # Obtain the predictive mean and standard deviation for each candidate point. + mu, sigma = gp_model.predict(candidate_points, return_std=True) + + # Compute the acquisition values based on the chosen method. + if method.lower() == "ucb": + # For maximization, UCB is defined as μ + kappa * σ. + acquisition = mu + kappa * sigma + elif method.lower() == "pi": + # Probability of Improvement (maximization formulation). + Z = (mu - 1.0 - xi) / (sigma + 1e-9) + acquisition = norm.cdf(Z) + elif method.lower() == "ei": + # Expected Improvement (maximization formulation). + Z = (mu - 1.0 - xi) / (sigma + 1e-9) + acquisition = (mu - 1.0 - xi) * norm.cdf(Z) + sigma * norm.pdf(Z) + # Handle nearly zero sigma values. + acquisition[sigma < 1e-9] = 0.0 + else: + raise ValueError("Unknown acquisition method. Please choose 'ucb', 'pi', or 'ei'.") + + # Sort candidates by acquisition value in descending order and select the top batch_size. + sorted_indices = np.argsort(acquisition)[::-1] + best_indices = sorted_indices[:batch_size] + best_candidates = candidate_points[best_indices] + + # Convert the numpy array to a torch tensor. + best_candidates_tensor = torch.from_numpy(best_candidates).to(device) + + return best_candidates_tensor, acquisition + + +""" +Util Functions +""" + + +def get_gripper_open_width(obj_filepath): + + retrieve_file_path(obj_filepath, download_dir="./") + obj_mesh = trimesh.load_mesh(os.path.basename(obj_filepath)) + # obj_mesh = trimesh.load_mesh(obj_filepath) + aabb = obj_mesh.bounds + + return min(0.04, (aabb[1][1] - aabb[0][1]) / 1.25) + + +""" +Imitation Reward +""" + + +def get_closest_state_idx(ref_traj, curr_ee_pos): + """Find the index of the closest state in reference trajectory.""" + + # ref_traj.shape = (num_trajs, traj_len, 3) + traj_len = ref_traj.shape[1] + num_envs = curr_ee_pos.shape[0] + + # dist_from_all_state.shape = (num_envs, num_trajs, traj_len, 1) + dist_from_all_state = torch.cdist(ref_traj.unsqueeze(0), curr_ee_pos.reshape(-1, 1, 1, 3), p=2) + + # dist_from_all_state_flatten.shape = (num_envs, num_trajs * traj_len) + dist_from_all_state_flatten = dist_from_all_state.reshape(num_envs, -1) + + # min_dist_per_env.shape = (num_envs) + min_dist_per_env = torch.amin(dist_from_all_state_flatten, dim=-1) + + # min_dist_idx.shape = (num_envs) + min_dist_idx = torch.argmin(dist_from_all_state_flatten, dim=-1) + + # min_dist_traj_idx.shape = (num_envs) + # min_dist_step_idx.shape = (num_envs) + min_dist_traj_idx = min_dist_idx // traj_len + min_dist_step_idx = min_dist_idx % traj_len + + return min_dist_traj_idx, min_dist_step_idx, min_dist_per_env + + +def get_reward_mask(ref_traj, curr_ee_pos, tolerance): + + _, min_dist_step_idx, _ = get_closest_state_idx(ref_traj, curr_ee_pos) + selected_steps = torch.index_select( + ref_traj, dim=1, index=min_dist_step_idx + ) # selected_steps.shape = (num_trajs, num_envs, 3) + + x_min = torch.amin(selected_steps[:, :, 0], dim=0) - tolerance + x_max = torch.amax(selected_steps[:, :, 0], dim=0) + tolerance + y_min = torch.amin(selected_steps[:, :, 1], dim=0) - tolerance + y_max = torch.amax(selected_steps[:, :, 1], dim=0) + tolerance + + x_in_range = torch.logical_and(torch.lt(curr_ee_pos[:, 0], x_max), torch.gt(curr_ee_pos[:, 0], x_min)) + y_in_range = torch.logical_and(torch.lt(curr_ee_pos[:, 1], y_max), torch.gt(curr_ee_pos[:, 1], y_min)) + pos_in_range = torch.logical_and(x_in_range, y_in_range).int() + + return pos_in_range + + +def get_imitation_reward_from_dtw(ref_traj, curr_ee_pos, prev_ee_traj, criterion, device): + """Get imitation reward based on dynamic time warping.""" + + soft_dtw = torch.zeros((curr_ee_pos.shape[0]), device=device) + prev_ee_pos = prev_ee_traj[:, 0, :] # select the first ee pos in robot traj + min_dist_traj_idx, min_dist_step_idx, min_dist_per_env = get_closest_state_idx(ref_traj, prev_ee_pos) + + for i in range(curr_ee_pos.shape[0]): + traj_idx = min_dist_traj_idx[i] + step_idx = min_dist_step_idx[i] + curr_ee_pos_i = curr_ee_pos[i].reshape(1, 3) + + # NOTE: in reference trajectories, larger index -> closer to goal + traj = ref_traj[traj_idx, step_idx:, :].reshape((1, -1, 3)) + + _, curr_step_idx, _ = get_closest_state_idx(traj, curr_ee_pos_i) + + if curr_step_idx == 0: + selected_pos = ref_traj[traj_idx, step_idx, :].reshape((1, 1, 3)) + selected_traj = torch.cat([selected_pos, selected_pos], dim=1) + else: + selected_traj = ref_traj[traj_idx, step_idx : (curr_step_idx + step_idx), :].reshape((1, -1, 3)) + eef_traj = torch.cat((prev_ee_traj[i, 1:, :], curr_ee_pos_i)).reshape((1, -1, 3)) + soft_dtw[i] = criterion(eef_traj, selected_traj) + + w_task_progress = 1 - (min_dist_step_idx / ref_traj.shape[1]) + + # imitation_rwd = torch.exp(-soft_dtw) + imitation_rwd = 1 - torch.tanh(soft_dtw) + + return imitation_rwd * w_task_progress + + +""" +Sampling-Based Curriculum (SBC) +""" + + +def get_new_max_disp(curr_success, cfg_task, curriculum_height_bound, curriculum_height_step, curr_max_disp): + """Update max downward displacement of plug at beginning of episode, based on success rate.""" + + if curr_success > cfg_task.curriculum_success_thresh: + # If success rate is above threshold, increase min downward displacement until max value + new_max_disp = torch.where( + curr_max_disp + curriculum_height_step[:, 0] < curriculum_height_bound[:, 1], + curr_max_disp + curriculum_height_step[:, 0], + curriculum_height_bound[:, 1], + ) + elif curr_success < cfg_task.curriculum_failure_thresh: + # If success rate is below threshold, decrease min downward displacement until min value + new_max_disp = torch.where( + curr_max_disp + curriculum_height_step[:, 1] > curriculum_height_bound[:, 0], + curr_max_disp + curriculum_height_step[:, 1], + curriculum_height_bound[:, 0], + ) + else: + # Maintain current max downward displacement + new_max_disp = curr_max_disp + + return new_max_disp + + +""" +Bonus and Success Checking +""" + + +def check_plug_close_to_socket(keypoints_plug, keypoints_socket, dist_threshold, progress_buf): + """Check if plug is close to socket.""" + + # Compute keypoint distance between plug and socket + keypoint_dist = torch.norm(keypoints_socket - keypoints_plug, p=2, dim=-1) + + # Check if keypoint distance is below threshold + is_plug_close_to_socket = torch.where( + torch.mean(keypoint_dist, dim=-1) < dist_threshold, + torch.ones_like(progress_buf), + torch.zeros_like(progress_buf), + ) + + return is_plug_close_to_socket + + +def check_plug_inserted_in_socket( + plug_pos, socket_pos, disassembly_dist, keypoints_plug, keypoints_socket, close_error_thresh, progress_buf +): + """Check if plug is inserted in socket.""" + + # Check if plug is within threshold distance of assembled state + is_plug_below_insertion_height = plug_pos[:, 2] < socket_pos[:, 2] + disassembly_dist + is_plug_above_table_height = plug_pos[:, 2] > socket_pos[:, 2] + + is_plug_height_success = torch.logical_and(is_plug_below_insertion_height, is_plug_above_table_height) + + # Check if plug is close to socket + # NOTE: This check addresses edge case where plug is within threshold distance of + # assembled state, but plug is outside socket + is_plug_close_to_socket = check_plug_close_to_socket( + keypoints_plug=keypoints_plug, + keypoints_socket=keypoints_socket, + dist_threshold=close_error_thresh, + progress_buf=progress_buf, + ) + + # Combine both checks + is_plug_inserted_in_socket = torch.logical_and(is_plug_height_success, is_plug_close_to_socket) + + return is_plug_inserted_in_socket + + +def get_curriculum_reward_scale(curr_max_disp, curriculum_height_bound): + """Compute reward scale for SBC.""" + + # Compute difference between max downward displacement at beginning of training (easiest condition) + # and current max downward displacement (based on current curriculum stage) + # NOTE: This number increases as curriculum gets harder + curr_stage_diff = curr_max_disp - curriculum_height_bound[:, 0] + + # Compute difference between max downward displacement at beginning of training (easiest condition) + # and min downward displacement (hardest condition) + final_stage_diff = curriculum_height_bound[:, 1] - curriculum_height_bound[:, 0] + + # Compute reward scale + reward_scale = curr_stage_diff / final_stage_diff + 1.0 + + return reward_scale.mean() + + +""" +Warp Kernels +""" + + +# Transform points from source coordinate frame to destination coordinate frame +@wp.kernel +def transform_points(src: wp.array(dtype=wp.vec3), dest: wp.array(dtype=wp.vec3), xform: wp.transform): + tid = wp.tid() + + p = src[tid] + m = wp.transform_point(xform, p) + + dest[tid] = m + + +# Return interpenetration distances between query points (e.g., plug vertices in current pose) +# and mesh surfaces (e.g., of socket mesh in current pose) +@wp.kernel +def get_interpen_dist( + queries: wp.array(dtype=wp.vec3), + mesh: wp.uint64, + interpen_dists: wp.array(dtype=wp.float32), +): + tid = wp.tid() + + # Declare arguments to wp.mesh_query_point() that will not be modified + q = queries[tid] # query point + max_dist = 1.5 # max distance on mesh from query point + + # Declare arguments to wp.mesh_query_point() that will be modified + sign = float( + 0.0 + ) # -1 if query point inside mesh; 0 if on mesh; +1 if outside mesh (NOTE: Mesh must be watertight!) + face_idx = int(0) # index of closest face + face_u = float(0.0) # barycentric u-coordinate of closest point + face_v = float(0.0) # barycentric v-coordinate of closest point + + # Get closest point on mesh to query point + closest_mesh_point_exists = wp.mesh_query_point(mesh, q, max_dist, sign, face_idx, face_u, face_v) + + # If point exists within max_dist + if closest_mesh_point_exists: + # Get 3D position of point on mesh given face index and barycentric coordinates + p = wp.mesh_eval_position(mesh, face_idx, face_u, face_v) + + # Get signed distance between query point and mesh point + delta = q - p + signed_dist = sign * wp.length(delta) + + # If signed distance is negative + if signed_dist < 0.0: + # Store interpenetration distance + interpen_dists[tid] = signed_dist diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_log_utils.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_log_utils.py new file mode 100644 index 00000000000..527680a1b6b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/automate_log_utils.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import h5py + + +def write_log_to_hdf5(held_asset_pose_log, fixed_asset_pose_log, success_log, eval_logging_filename): + + with h5py.File(eval_logging_filename, "w") as hf: + hf.create_dataset("held_asset_pose", data=held_asset_pose_log.cpu().numpy()) + hf.create_dataset("fixed_asset_pose", data=fixed_asset_pose_log.cpu().numpy()) + hf.create_dataset("success", data=success_log.cpu().numpy()) + + +def load_log_from_hdf5(eval_logging_filename): + + with h5py.File(eval_logging_filename, "r") as hf: + held_asset_pose = hf["held_asset_pose"][:] + fixed_asset_pose = hf["fixed_asset_pose"][:] + success = hf["success"][:] + + # held_asset_pose = torch.from_numpy(held_asset_pose).to(device) + # fixed_asset_pose = torch.from_numpy(fixed_asset_pose).to(device) + # success = torch.from_numpy(success).to(device) + + return held_asset_pose, fixed_asset_pose, success diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env.py new file mode 100644 index 00000000000..8f98b7aaabb --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env.py @@ -0,0 +1,890 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import json +import numpy as np +import os +import torch + +import carb +import isaacsim.core.utils.torch as torch_utils + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.math import axis_angle_from_quat + +from . import automate_algo_utils as automate_algo +from . import factory_control as fc +from .disassembly_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, DisassemblyEnvCfg + + +class DisassemblyEnv(DirectRLEnv): + cfg: DisassemblyEnvCfg + + def __init__(self, cfg: DisassemblyEnvCfg, render_mode: str | None = None, **kwargs): + + # Update number of obs/states + cfg.observation_space = sum([OBS_DIM_CFG[obs] for obs in cfg.obs_order]) + cfg.state_space = sum([STATE_DIM_CFG[state] for state in cfg.state_order]) + self.cfg_task = cfg.tasks[cfg.task_name] + + super().__init__(cfg, render_mode, **kwargs) + + self._set_body_inertias() + self._init_tensors() + self._set_default_dynamics_parameters() + self._compute_intermediate_values(dt=self.physics_dt) + + # Get the gripper open width based on plug object bounding box + self.gripper_open_width = automate_algo.get_gripper_open_width( + self.cfg_task.assembly_dir + self.cfg_task.held_asset_cfg.obj_path + ) + + # initialized logging variables for disassembly paths + self._init_log_data_per_assembly() + + def _set_body_inertias(self): + """Note: this is to account for the asset_options.armature parameter in IGE.""" + inertias = self._robot.root_physx_view.get_inertias() + offset = torch.zeros_like(inertias) + offset[:, :, [0, 4, 8]] += 0.01 + new_inertias = inertias + offset + self._robot.root_physx_view.set_inertias(new_inertias, torch.arange(self.num_envs)) + + def _set_default_dynamics_parameters(self): + """Set parameters defining dynamic interactions.""" + self.default_gains = torch.tensor(self.cfg.ctrl.default_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + + self.pos_threshold = torch.tensor(self.cfg.ctrl.pos_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + self.rot_threshold = torch.tensor(self.cfg.ctrl.rot_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + + # Set masses and frictions. + self._set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction) + self._set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction) + self._set_friction(self._robot, self.cfg_task.robot_cfg.friction) + + def _set_friction(self, asset, value): + """Update material properties for a given asset.""" + materials = asset.root_physx_view.get_material_properties() + materials[..., 0] = value # Static friction. + materials[..., 1] = value # Dynamic friction. + env_ids = torch.arange(self.scene.num_envs, device="cpu") + asset.root_physx_view.set_material_properties(materials, env_ids) + + def _init_tensors(self): + """Initialize tensors once.""" + self.identity_quat = ( + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) + + # Control targets. + self.ctrl_target_joint_pos = torch.zeros((self.num_envs, self._robot.num_joints), device=self.device) + self.ctrl_target_fingertip_midpoint_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.ctrl_target_fingertip_midpoint_quat = torch.zeros((self.num_envs, 4), device=self.device) + + # Fixed asset. + self.fixed_pos_action_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.fixed_pos_obs_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.init_fixed_pos_obs_noise = torch.zeros((self.num_envs, 3), device=self.device) + + # Held asset + held_base_x_offset = 0.0 + held_base_z_offset = 0.0 + + self.held_base_pos_local = torch.tensor([0.0, 0.0, 0.0], device=self.device).repeat((self.num_envs, 1)) + self.held_base_pos_local[:, 0] = held_base_x_offset + self.held_base_pos_local[:, 2] = held_base_z_offset + self.held_base_quat_local = self.identity_quat.clone().detach() + + self.held_base_pos = torch.zeros_like(self.held_base_pos_local) + self.held_base_quat = self.identity_quat.clone().detach() + + self.plug_grasps, self.disassembly_dists = self._load_assembly_info() + + # Load grasp pose from json files given assembly ID + # Grasp pose tensors + self.palm_to_finger_center = ( + torch.tensor([0.0, 0.0, -self.cfg_task.palm_to_finger_dist], device=self.device) + .unsqueeze(0) + .repeat(self.num_envs, 1) + ) + self.robot_to_gripper_quat = ( + torch.tensor([0.0, 1.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) + self.plug_grasp_pos_local = self.plug_grasps[: self.num_envs, :3] + self.plug_grasp_quat_local = torch.roll(self.plug_grasps[: self.num_envs, 3:], -1, 1) + + # Computer body indices. + self.left_finger_body_idx = self._robot.body_names.index("panda_leftfinger") + self.right_finger_body_idx = self._robot.body_names.index("panda_rightfinger") + self.fingertip_body_idx = self._robot.body_names.index("panda_fingertip_centered") + + # Tensors for finite-differencing. + self.last_update_timestamp = 0.0 # Note: This is for finite differencing body velocities. + self.prev_fingertip_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.prev_fingertip_quat = self.identity_quat.clone() + self.prev_joint_pos = torch.zeros((self.num_envs, 7), device=self.device) + + # Keypoint tensors. + self.target_held_base_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.target_held_base_quat = self.identity_quat.clone().detach() + + # Used to compute target poses. + self.fixed_success_pos_local = torch.zeros((self.num_envs, 3), device=self.device) + self.fixed_success_pos_local[:, 2] = 0.0 + + self.ep_succeeded = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + self.ep_success_times = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + + def _load_assembly_info(self): + """Load grasp pose and disassembly distance for plugs in each environment.""" + + retrieve_file_path(self.cfg_task.plug_grasp_json, download_dir="./") + with open(os.path.basename(self.cfg_task.plug_grasp_json)) as f: + plug_grasp_dict = json.load(f) + plug_grasps = [plug_grasp_dict[f"asset_{self.cfg_task.assembly_id}"] for i in range(self.num_envs)] + + retrieve_file_path(self.cfg_task.disassembly_dist_json, download_dir="./") + with open(os.path.basename(self.cfg_task.disassembly_dist_json)) as f: + disassembly_dist_dict = json.load(f) + disassembly_dists = [disassembly_dist_dict[f"asset_{self.cfg_task.assembly_id}"] for i in range(self.num_envs)] + + return torch.as_tensor(plug_grasps).to(self.device), torch.as_tensor(disassembly_dists).to(self.device) + + def _setup_scene(self): + """Initialize simulation scene.""" + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg(), translation=(0.0, 0.0, -0.4)) + + # spawn a usd file of a table into the scene + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") + cfg.func( + "/World/envs/env_.*/Table", cfg, translation=(0.55, 0.0, 0.0), orientation=(0.70711, 0.0, 0.0, 0.70711) + ) + + self._robot = Articulation(self.cfg.robot) + self._fixed_asset = Articulation(self.cfg_task.fixed_asset) + # self._held_asset = Articulation(self.cfg_task.held_asset) + # self._fixed_asset = RigidObject(self.cfg_task.fixed_asset) + self._held_asset = RigidObject(self.cfg_task.held_asset) + + self.scene.clone_environments(copy_from_source=False) + self.scene.filter_collisions() + + self.scene.articulations["robot"] = self._robot + self.scene.articulations["fixed_asset"] = self._fixed_asset + # self.scene.articulations["held_asset"] = self._held_asset + # self.scene.rigid_objects["fixed_asset"] = self._fixed_asset + self.scene.rigid_objects["held_asset"] = self._held_asset + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _compute_intermediate_values(self, dt): + """Get values computed from raw tensors. This includes adding noise.""" + # TODO: A lot of these can probably only be set once? + self.fixed_pos = self._fixed_asset.data.root_pos_w - self.scene.env_origins + self.fixed_quat = self._fixed_asset.data.root_quat_w + + self.held_pos = self._held_asset.data.root_pos_w - self.scene.env_origins + self.held_quat = self._held_asset.data.root_quat_w + + self.fingertip_midpoint_pos = self._robot.data.body_pos_w[:, self.fingertip_body_idx] - self.scene.env_origins + self.fingertip_midpoint_quat = self._robot.data.body_quat_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w[:, self.fingertip_body_idx] + + jacobians = self._robot.root_physx_view.get_jacobians() + + self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] + self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] + self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 + self.arm_mass_matrix = self._robot.root_physx_view.get_generalized_mass_matrices()[:, 0:7, 0:7] + self.joint_pos = self._robot.data.joint_pos.clone() + self.joint_vel = self._robot.data.joint_vel.clone() + + # Compute pose of gripper goal and top of socket in socket frame + self.gripper_goal_quat, self.gripper_goal_pos = torch_utils.tf_combine( + self.fixed_quat, + self.fixed_pos, + self.plug_grasp_quat_local, + self.plug_grasp_pos_local, + ) + + self.gripper_goal_quat, self.gripper_goal_pos = torch_utils.tf_combine( + self.gripper_goal_quat, + self.gripper_goal_pos, + self.robot_to_gripper_quat, + self.palm_to_finger_center, + ) + + # Finite-differencing results in more reliable velocity estimates. + self.ee_linvel_fd = (self.fingertip_midpoint_pos - self.prev_fingertip_pos) / dt + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + + # Add state differences if velocity isn't being added. + rot_diff_quat = torch_utils.quat_mul( + self.fingertip_midpoint_quat, torch_utils.quat_conjugate(self.prev_fingertip_quat) + ) + rot_diff_quat *= torch.sign(rot_diff_quat[:, 0]).unsqueeze(-1) + rot_diff_aa = axis_angle_from_quat(rot_diff_quat) + self.ee_angvel_fd = rot_diff_aa / dt + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + joint_diff = self.joint_pos[:, 0:7] - self.prev_joint_pos + self.joint_vel_fd = joint_diff / dt + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + + # Keypoint tensors. + self.held_base_quat[:], self.held_base_pos[:] = torch_utils.tf_combine( + self.held_quat, self.held_pos, self.held_base_quat_local, self.held_base_pos_local + ) + self.target_held_base_quat[:], self.target_held_base_pos[:] = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, self.fixed_success_pos_local + ) + + self.last_update_timestamp = self._robot._data._sim_timestamp + + def _get_observations(self): + """Get actor/critic inputs using asymmetric critic.""" + + obs_dict = { + "joint_pos": self.joint_pos[:, 0:7], + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_quat": self.fingertip_midpoint_quat, + "fingertip_goal_pos": self.gripper_goal_pos, + "fingertip_goal_quat": self.gripper_goal_quat, + "delta_pos": self.gripper_goal_pos - self.fingertip_midpoint_pos, + } + + state_dict = { + "joint_pos": self.joint_pos[:, 0:7], + "joint_vel": self.joint_vel[:, 0:7], + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_quat": self.fingertip_midpoint_quat, + "ee_linvel": self.fingertip_midpoint_linvel, + "ee_angvel": self.fingertip_midpoint_angvel, + "fingertip_goal_pos": self.gripper_goal_pos, + "fingertip_goal_quat": self.gripper_goal_quat, + "held_pos": self.held_pos, + "held_quat": self.held_quat, + "delta_pos": self.gripper_goal_pos - self.fingertip_midpoint_pos, + } + # obs_tensors = [obs_dict[obs_name] for obs_name in self.cfg.obs_order + ['prev_actions']] + obs_tensors = [obs_dict[obs_name] for obs_name in self.cfg.obs_order] + obs_tensors = torch.cat(obs_tensors, dim=-1) + + # state_tensors = [state_dict[state_name] for state_name in self.cfg.state_order + ['prev_actions']] + state_tensors = [state_dict[state_name] for state_name in self.cfg.state_order] + state_tensors = torch.cat(state_tensors, dim=-1) + + return {"policy": obs_tensors, "critic": state_tensors} + + def _reset_buffers(self, env_ids): + """Reset buffers.""" + self.ep_succeeded[env_ids] = 0 + + def _pre_physics_step(self, action): + """Apply policy actions with smoothing.""" + env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(env_ids) > 0: + self._reset_buffers(env_ids) + + def move_gripper_in_place(self, ctrl_target_gripper_dof_pos): + """Keep gripper in current position as gripper closes.""" + actions = torch.zeros((self.num_envs, 6), device=self.device) + ctrl_target_gripper_dof_pos = 0.0 + + # Interpret actions as target pos displacements and set pos target + pos_actions = actions[:, 0:3] * self.pos_threshold + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = actions[:, 3:6] + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1.0e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = ctrl_target_gripper_dof_pos + self.generate_ctrl_signals() + + def _apply_action(self): + """Apply actions for policy as delta targets from current position.""" + # Get current yaw for success checking. + _, _, curr_yaw = torch_utils.get_euler_xyz(self.fingertip_midpoint_quat) + self.curr_yaw = torch.where(curr_yaw > np.deg2rad(235), curr_yaw - 2 * np.pi, curr_yaw) + + # Note: We use finite-differenced velocities for control and observations. + # Check if we need to re-compute velocities within the decimation loop. + if self.last_update_timestamp < self._robot._data._sim_timestamp: + self._compute_intermediate_values(dt=self.physics_dt) + + # Interpret actions as target pos displacements and set pos target + pos_actions = self.actions[:, 0:3] * self.pos_threshold + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = self.actions[:, 3:6] + if self.cfg_task.unidirectional_rot: + rot_actions[:, 2] = -(rot_actions[:, 2] + 1.0) * 0.5 # [-1, 0] + rot_actions = rot_actions * self.rot_threshold + + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + # To speed up learning, never allow the policy to move more than 5cm away from the base. + delta_pos = self.ctrl_target_fingertip_midpoint_pos - self.fixed_pos_action_frame + pos_error_clipped = torch.clip( + delta_pos, -self.cfg.ctrl.pos_action_bounds[0], self.cfg.ctrl.pos_action_bounds[1] + ) + self.ctrl_target_fingertip_midpoint_pos = self.fixed_pos_action_frame + pos_error_clipped + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 # Restrict actions to be upright. + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = 0.0 + self.generate_ctrl_signals() + + def _set_gains(self, prop_gains, rot_deriv_scale=1.0): + """Set robot gains using critical damping.""" + self.task_prop_gains = prop_gains + self.task_deriv_gains = 2 * torch.sqrt(prop_gains) + self.task_deriv_gains[:, 3:6] /= rot_deriv_scale + + def generate_ctrl_signals(self): + """Get Jacobian. Set Franka DOF position targets (fingers) or DOF torques (arm).""" + self.joint_torque, self.applied_wrench = fc.compute_dof_torque( + cfg=self.cfg, + dof_pos=self.joint_pos, + dof_vel=self.joint_vel, # _fd, + fingertip_midpoint_pos=self.fingertip_midpoint_pos, + fingertip_midpoint_quat=self.fingertip_midpoint_quat, + fingertip_midpoint_linvel=self.ee_linvel_fd, + fingertip_midpoint_angvel=self.ee_angvel_fd, + jacobian=self.fingertip_midpoint_jacobian, + arm_mass_matrix=self.arm_mass_matrix, + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat, + task_prop_gains=self.task_prop_gains, + task_deriv_gains=self.task_deriv_gains, + device=self.device, + ) + + # set target for gripper joints to use GYM's PD controller + self.ctrl_target_joint_pos[:, 7:9] = self.ctrl_target_gripper_dof_pos + self.joint_torque[:, 7:9] = 0.0 + + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + self._robot.set_joint_effort_target(self.joint_torque) + + def _get_dones(self): + """Update intermediate values used for rewards and observations.""" + self._compute_intermediate_values(dt=self.physics_dt) + time_out = self.episode_length_buf >= self.max_episode_length - 1 + + if time_out[0]: + + self.close_gripper(env_ids=np.array(range(self.num_envs)).reshape(-1)) + self._disassemble_plug_from_socket() + + if_intersect = (self.held_pos[:, 2] < self.fixed_pos[:, 2] + self.disassembly_dists).cpu().numpy() + success_env_ids = np.argwhere(if_intersect == 0).reshape(-1) + + self._log_robot_state(success_env_ids) + self._log_object_state(success_env_ids) + self._save_log_traj() + + return time_out, time_out + + def _get_rewards(self): + """Update rewards and compute success statistics.""" + # Get successful and failed envs at current timestep + + rew_buf = self._update_rew_buf() + return rew_buf + + def _update_rew_buf(self): + """Compute reward at current timestep.""" + return torch.zeros((self.num_envs,), device=self.device) + + def _reset_idx(self, env_ids): + """ + We assume all envs will always be reset at the same time. + """ + super()._reset_idx(env_ids) + + self._set_assets_to_default_pose(env_ids) + self._set_franka_to_default_pose(joints=self.cfg.ctrl.reset_joints, env_ids=env_ids) + self.step_sim_no_action() + + self.randomize_initial_state(env_ids) + + prev_fingertip_midpoint_pos = (self.fingertip_midpoint_pos - self.gripper_goal_pos).unsqueeze( + 1 + ) # (num_envs, 1, 3) + self.prev_fingertip_midpoint_pos = torch.repeat_interleave( + prev_fingertip_midpoint_pos, self.cfg_task.num_point_robot_traj, dim=1 + ) # (num_envs, num_point_robot_traj, 3) + self._init_log_data_per_episode() + + def _set_assets_to_default_pose(self, env_ids): + """Move assets to default pose before randomization.""" + held_state = self._held_asset.data.default_root_state.clone()[env_ids] + held_state[:, 0:3] += self.scene.env_origins[env_ids] + held_state[:, 7:] = 0.0 + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7], env_ids=env_ids) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:], env_ids=env_ids) + self._held_asset.reset() + + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + fixed_state[:, 0:3] += self.scene.env_origins[env_ids] + fixed_state[:, 7:] = 0.0 + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.reset() + + def _move_gripper_to_grasp_pose(self, env_ids): + """Define grasp pose for plug and move gripper to pose.""" + + gripper_goal_quat, gripper_goal_pos = torch_utils.tf_combine( + self.held_quat, + self.held_pos, + self.plug_grasp_quat_local, + self.plug_grasp_pos_local, + ) + + gripper_goal_quat, gripper_goal_pos = torch_utils.tf_combine( + gripper_goal_quat, + gripper_goal_pos, + self.robot_to_gripper_quat, + self.palm_to_finger_center, + ) + + # Set target_pos + self.ctrl_target_fingertip_midpoint_pos = gripper_goal_pos.clone() + + # Set target rot + self.ctrl_target_fingertip_midpoint_quat = gripper_goal_quat.clone() + + self.set_pos_inverse_kinematics(env_ids) + self.step_sim_no_action() + + def set_pos_inverse_kinematics(self, env_ids): + """Set robot joint position using DLS IK.""" + ik_time = 0.0 + while ik_time < 0.50: + # Compute error to target. + pos_error, axis_angle_error = fc.get_pose_error( + fingertip_midpoint_pos=self.fingertip_midpoint_pos[env_ids], + fingertip_midpoint_quat=self.fingertip_midpoint_quat[env_ids], + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos[env_ids], + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat[env_ids], + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + + delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) + + # Solve DLS problem. + delta_dof_pos = fc._get_delta_dof_pos( + delta_pose=delta_hand_pose, + ik_method="dls", + jacobian=self.fingertip_midpoint_jacobian[env_ids], + device=self.device, + ) + self.joint_pos[env_ids, 0:7] += delta_dof_pos[:, 0:7] + self.joint_vel[env_ids, :] = torch.zeros_like(self.joint_pos[env_ids,]) + + self.ctrl_target_joint_pos[env_ids, 0:7] = self.joint_pos[env_ids, 0:7] + # Update dof state. + self._robot.write_joint_state_to_sim(self.joint_pos, self.joint_vel) + self._robot.reset() + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + + # Simulate and update tensors. + self.step_sim_no_action() + ik_time += self.physics_dt + + return pos_error, axis_angle_error + + def _move_gripper_to_eef_pose(self, env_ids, goal_pos, goal_quat, sim_steps, if_log=False): + + for _ in range(sim_steps): + if if_log: + self._log_robot_state_per_timestep() + # print('finger', self.fingertip_midpoint_pos[0], 'goal', goal_pos[0]) + # Compute error to target. + pos_error, axis_angle_error = fc.get_pose_error( + fingertip_midpoint_pos=self.fingertip_midpoint_pos[env_ids], + fingertip_midpoint_quat=self.fingertip_midpoint_quat[env_ids], + ctrl_target_fingertip_midpoint_pos=goal_pos[env_ids], + ctrl_target_fingertip_midpoint_quat=goal_quat[env_ids], + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + + delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) + # print('delta hand pose', delta_hand_pose[0]) + self.actions *= 0.0 + # print('action shape', self.actions[env_ids, :6].shape) + # print('delta hand shape', delta_hand_pose.shape) + self.actions[env_ids, :6] = delta_hand_pose + + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self._apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # Simulate and update tensors. + self.step_sim_no_action() + + def _set_franka_to_default_pose(self, joints, env_ids): + """Return Franka to its default joint position.""" + # gripper_width = self.cfg_task.held_asset_cfg.diameter / 2 * 1.25 + # gripper_width = self.cfg_task.hand_width_max / 3.0 + gripper_width = self.gripper_open_width + joint_pos = self._robot.data.default_joint_pos[env_ids] + joint_pos[:, 7:] = gripper_width # MIMIC + joint_pos[:, :7] = torch.tensor(joints, device=self.device)[None, :] + joint_vel = torch.zeros_like(joint_pos) + joint_effort = torch.zeros_like(joint_pos) + self.ctrl_target_joint_pos[env_ids, :] = joint_pos + self._robot.set_joint_position_target(self.ctrl_target_joint_pos[env_ids], env_ids=env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + self._robot.reset() + self._robot.set_joint_effort_target(joint_effort, env_ids=env_ids) + + self.step_sim_no_action() + + def step_sim_no_action(self): + """Step the simulation without an action. Used for resets.""" + self.scene.write_data_to_sim() + self.sim.step(render=True) + self.scene.update(dt=self.physics_dt) + self._compute_intermediate_values(dt=self.physics_dt) + + def randomize_fixed_initial_state(self, env_ids): + + # (1.) Randomize fixed asset pose. + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + # (1.a.) Position + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + fixed_asset_init_pos_rand = torch.tensor( + self.cfg_task.fixed_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + fixed_pos_init_rand = fixed_pos_init_rand @ torch.diag(fixed_asset_init_pos_rand) + fixed_state[:, 0:3] += fixed_pos_init_rand + self.scene.env_origins[env_ids] + fixed_state[:, 2] += self.cfg_task.fixed_asset_z_offset + + # (1.b.) Orientation + fixed_orn_init_yaw = np.deg2rad(self.cfg_task.fixed_asset_init_orn_deg) + fixed_orn_yaw_range = np.deg2rad(self.cfg_task.fixed_asset_init_orn_range_deg) + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_orn_euler = fixed_orn_init_yaw + fixed_orn_yaw_range * rand_sample + fixed_orn_euler[:, 0:2] = 0.0 # Only change yaw. + fixed_orn_quat = torch_utils.quat_from_euler_xyz( + fixed_orn_euler[:, 0], fixed_orn_euler[:, 1], fixed_orn_euler[:, 2] + ) + fixed_state[:, 3:7] = fixed_orn_quat + # (1.c.) Velocity + fixed_state[:, 7:] = 0.0 # vel + # (1.d.) Update values. + self._fixed_asset.write_root_state_to_sim(fixed_state, env_ids=env_ids) + self._fixed_asset.reset() + + # (1.e.) Noisy position observation. + fixed_asset_pos_noise = torch.randn((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_asset_pos_rand = torch.tensor(self.cfg.obs_rand.fixed_asset_pos, dtype=torch.float32, device=self.device) + fixed_asset_pos_noise = fixed_asset_pos_noise @ torch.diag(fixed_asset_pos_rand) + self.init_fixed_pos_obs_noise[:] = fixed_asset_pos_noise + + self.step_sim_no_action() + + def randomize_held_initial_state(self, env_ids, pre_grasp): + + # Set plug pos to assembled state + held_state = self._held_asset.data.default_root_state.clone() + held_state[env_ids, 0:3] = self.fixed_pos[env_ids].clone() + self.scene.env_origins[env_ids] + held_state[env_ids, 3:7] = self.fixed_quat[env_ids].clone() + held_state[env_ids, 7:] = 0.0 + + self._held_asset.write_root_state_to_sim(held_state) + self._held_asset.reset() + + self.step_sim_no_action() + + def close_gripper(self, env_ids): + # Close hand + # Set gains to use for quick resets. + reset_task_prop_gains = torch.tensor(self.cfg.ctrl.reset_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + reset_rot_deriv_scale = self.cfg.ctrl.reset_rot_deriv_scale + self._set_gains(reset_task_prop_gains, reset_rot_deriv_scale) + + self.step_sim_no_action() + + grasp_time = 0.0 + while grasp_time < 0.25: + self.ctrl_target_joint_pos[env_ids, 7:] = 0.0 # Close gripper. + self.ctrl_target_gripper_dof_pos = 0.0 + self.move_gripper_in_place(ctrl_target_gripper_dof_pos=0.0) + self.step_sim_no_action() + grasp_time += self.sim.get_physics_dt() + + def randomize_initial_state(self, env_ids): + """Randomize initial state and perform any episode-level randomization.""" + # Disable gravity. + physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) + + self.randomize_fixed_initial_state(env_ids) + + # Compute the frame on the bolt that would be used as observation: fixed_pos_obs_frame + # For example, the tip of the bolt can be used as the observation frame + fixed_tip_pos_local = torch.zeros_like(self.fixed_pos) + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.height + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.base_height + + _, fixed_tip_pos = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, fixed_tip_pos_local + ) + self.fixed_pos_obs_frame[:] = fixed_tip_pos + + self.randomize_held_initial_state(env_ids, pre_grasp=True) + + self._move_gripper_to_grasp_pose(env_ids) + + self.randomize_held_initial_state(env_ids, pre_grasp=False) + + self.close_gripper(env_ids) + + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + # Set initial actions to involve no-movement. Needed for EMA/correct penalties. + self.actions = torch.zeros_like(self.actions) + self.prev_actions = torch.zeros_like(self.actions) + self.fixed_pos_action_frame[:] = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + + # Zero initial velocity. + self.ee_angvel_fd[:, :] = 0.0 + self.ee_linvel_fd[:, :] = 0.0 + + # Set initial gains for the episode. + self._set_gains(self.default_gains) + + physics_sim_view.set_gravity(carb.Float3(*self.cfg.sim.gravity)) + + def _disassemble_plug_from_socket(self): + """Lift plug from socket till disassembly and then randomize end-effector pose.""" + + if_intersect = np.ones(self.num_envs, dtype=np.float32) + + env_ids = np.argwhere(if_intersect == 1).reshape(-1) + self._lift_gripper(self.disassembly_dists * 3.0, self.cfg_task.disassemble_sim_steps, env_ids) + + self.step_sim_no_action() + + if_intersect = (self.held_pos[:, 2] < self.fixed_pos[:, 2] + self.disassembly_dists).cpu().numpy() + env_ids = np.argwhere(if_intersect == 0).reshape(-1) + # print('env ids', env_ids) + self._randomize_gripper_pose(self.cfg_task.move_gripper_sim_steps, env_ids) + + def _lift_gripper(self, lift_distance, sim_steps, env_ids=None): + """Lift gripper by specified distance. Called outside RL loop (i.e., after last step of episode).""" + + ctrl_tgt_pos = torch.empty_like(self.fingertip_midpoint_pos).copy_(self.fingertip_midpoint_pos) + # ctrl_tgt_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], dtype=torch.float32, device=self.device).repeat((self.num_envs,1)) + ctrl_tgt_quat = torch.empty_like(self.fingertip_midpoint_quat).copy_(self.fingertip_midpoint_quat) + ctrl_tgt_pos[:, 2] += lift_distance + if len(env_ids) == 0: + env_ids = np.array(range(self.num_envs)).reshape(-1) + + self._move_gripper_to_eef_pose(env_ids, ctrl_tgt_pos, ctrl_tgt_quat, sim_steps, if_log=True) + + def _randomize_gripper_pose(self, sim_steps, env_ids): + """Move gripper to random pose.""" + + ctrl_tgt_pos = torch.empty_like(self.gripper_goal_pos).copy_(self.gripper_goal_pos) + ctrl_tgt_pos[:, 2] += self.cfg_task.gripper_rand_z_offset + + # ctrl_tgt_pos = torch.empty_like(self.fingertip_midpoint_pos).copy_(self.fingertip_midpoint_pos) + + fingertip_centered_pos_noise = 2 * ( + torch.rand((self.num_envs, 3), dtype=torch.float32, device=self.device) - 0.5 + ) # [-1, 1] + fingertip_centered_pos_noise = fingertip_centered_pos_noise @ torch.diag( + torch.tensor(self.cfg_task.gripper_rand_pos_noise, device=self.device) + ) + ctrl_tgt_pos += fingertip_centered_pos_noise + + # Set target rot + ctrl_target_fingertip_centered_euler = ( + torch.tensor(self.cfg_task.fingertip_centered_rot_initial, device=self.device) + .unsqueeze(0) + .repeat(self.num_envs, 1) + ) + + fingertip_centered_rot_noise = 2 * ( + torch.rand((self.num_envs, 3), dtype=torch.float32, device=self.device) - 0.5 + ) # [-1, 1] + fingertip_centered_rot_noise = fingertip_centered_rot_noise @ torch.diag( + torch.tensor(self.cfg_task.gripper_rand_rot_noise, device=self.device) + ) + ctrl_target_fingertip_centered_euler += fingertip_centered_rot_noise + ctrl_tgt_quat = torch_utils.quat_from_euler_xyz( + ctrl_target_fingertip_centered_euler[:, 0], + ctrl_target_fingertip_centered_euler[:, 1], + ctrl_target_fingertip_centered_euler[:, 2], + ) + + # ctrl_tgt_quat = torch.empty_like(self.fingertip_midpoint_quat).copy_(self.fingertip_midpoint_quat) + + self._move_gripper_to_eef_pose(env_ids, ctrl_tgt_pos, ctrl_tgt_quat, sim_steps, if_log=True) + + def _init_log_data_per_assembly(self): + + self.log_assembly_id = [] + self.log_plug_pos = [] + self.log_plug_quat = [] + self.log_init_plug_pos = [] + self.log_init_plug_quat = [] + self.log_plug_grasp_pos = [] + self.log_plug_grasp_quat = [] + self.log_fingertip_centered_pos = [] + self.log_fingertip_centered_quat = [] + self.log_arm_dof_pos = [] + + def _init_log_data_per_episode(self): + + self.log_fingertip_centered_pos_traj = [] + self.log_fingertip_centered_quat_traj = [] + self.log_arm_dof_pos_traj = [] + self.log_plug_pos_traj = [] + self.log_plug_quat_traj = [] + + self.init_plug_grasp_pos = self.gripper_goal_pos.clone().detach() + self.init_plug_grasp_quat = self.gripper_goal_quat.clone().detach() + self.init_plug_pos = self.held_pos.clone().detach() + self.init_plug_quat = self.held_quat.clone().detach() + + def _log_robot_state(self, env_ids): + + self.log_plug_pos += torch.stack(self.log_plug_pos_traj, dim=1)[env_ids].cpu().tolist() + self.log_plug_quat += torch.stack(self.log_plug_quat_traj, dim=1)[env_ids].cpu().tolist() + self.log_arm_dof_pos += torch.stack(self.log_arm_dof_pos_traj, dim=1)[env_ids].cpu().tolist() + self.log_fingertip_centered_pos += ( + torch.stack(self.log_fingertip_centered_pos_traj, dim=1)[env_ids].cpu().tolist() + ) + self.log_fingertip_centered_quat += ( + torch.stack(self.log_fingertip_centered_quat_traj, dim=1)[env_ids].cpu().tolist() + ) + + def _log_robot_state_per_timestep(self): + + self.log_plug_pos_traj.append(self.held_pos.clone().detach()) + self.log_plug_quat_traj.append(self.held_quat.clone().detach()) + self.log_arm_dof_pos_traj.append(self.joint_pos[:, 0:7].clone().detach()) + self.log_fingertip_centered_pos_traj.append(self.fingertip_midpoint_pos.clone().detach()) + self.log_fingertip_centered_quat_traj.append(self.fingertip_midpoint_quat.clone().detach()) + + def _log_object_state(self, env_ids): + + self.log_plug_grasp_pos += self.init_plug_grasp_pos[env_ids].cpu().tolist() + self.log_plug_grasp_quat += self.init_plug_grasp_quat[env_ids].cpu().tolist() + self.log_init_plug_pos += self.init_plug_pos[env_ids].cpu().tolist() + self.log_init_plug_quat += self.init_plug_quat[env_ids].cpu().tolist() + + def _save_log_traj(self): + + if len(self.log_arm_dof_pos) > self.cfg_task.num_log_traj: + + log_item = [] + for i in range(self.cfg_task.num_log_traj): + curr_dict = dict({}) + curr_dict["fingertip_centered_pos"] = self.log_fingertip_centered_pos[i] + curr_dict["fingertip_centered_quat"] = self.log_fingertip_centered_quat[i] + curr_dict["arm_dof_pos"] = self.log_arm_dof_pos[i] + curr_dict["plug_grasp_pos"] = self.log_plug_grasp_pos[i] + curr_dict["plug_grasp_quat"] = self.log_plug_grasp_quat[i] + curr_dict["init_plug_pos"] = self.log_init_plug_pos[i] + curr_dict["init_plug_quat"] = self.log_init_plug_quat[i] + curr_dict["plug_pos"] = self.log_plug_pos[i] + curr_dict["plug_quat"] = self.log_plug_quat[i] + + log_item.append(curr_dict) + + log_filename = os.path.join( + os.getcwd(), self.cfg_task.disassembly_dir, self.cfg_task.assembly_id + "_disassemble_traj.json" + ) + + with open(log_filename, "w+") as out_file: + json.dump(log_item, out_file, indent=6) + + exit(0) + else: + print("current logging item num: ", len(self.log_arm_dof_pos)) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env_cfg.py new file mode 100644 index 00000000000..74bfee07817 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_env_cfg.py @@ -0,0 +1,201 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass + +from .disassembly_tasks_cfg import ASSET_DIR, Extraction + +OBS_DIM_CFG = { + "joint_pos": 7, + "fingertip_pos": 3, + "fingertip_quat": 4, + "fingertip_goal_pos": 3, + "fingertip_goal_quat": 4, + "delta_pos": 3, +} + +STATE_DIM_CFG = { + "joint_pos": 7, + "joint_vel": 7, + "fingertip_pos": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, + "fingertip_goal_pos": 3, + "fingertip_goal_quat": 4, + "held_pos": 3, + "held_quat": 4, + "delta_pos": 3, +} + + +@configclass +class ObsRandCfg: + fixed_asset_pos = [0.001, 0.001, 0.001] + + +@configclass +class CtrlCfg: + ema_factor = 0.2 + + pos_action_bounds = [0.1, 0.1, 0.1] + rot_action_bounds = [0.01, 0.01, 0.01] + + pos_action_threshold = [0.01, 0.01, 0.01] + rot_action_threshold = [0.01, 0.01, 0.01] + + reset_joints = [0.0, 0.0, 0.0, -1.870, 0.0, 1.8675, 0.785398] + reset_task_prop_gains = [1000, 1000, 1000, 50, 50, 50] + # reset_rot_deriv_scale = 1.0 + # default_task_prop_gains = [1000, 1000, 1000, 50, 50, 50] + # reset_task_prop_gains = [300, 300, 300, 20, 20, 20] + reset_rot_deriv_scale = 10.0 + default_task_prop_gains = [100, 100, 100, 30, 30, 30] + + # Null space parameters. + default_dof_pos_tensor = [0.0, 0.0, 0.0, -1.870, 0.0, 1.8675, 0.785398] + kp_null = 10.0 + kd_null = 6.3246 + + +@configclass +class DisassemblyEnvCfg(DirectRLEnvCfg): + decimation = 8 + action_space = 6 + # num_*: will be overwritten to correspond to obs_order, state_order. + observation_space = 24 + state_space = 44 + obs_order: list = [ + "joint_pos", + "fingertip_pos", + "fingertip_quat", + "fingertip_goal_pos", + "fingertip_goal_quat", + "delta_pos", + ] + state_order: list = [ + "joint_pos", + "joint_vel", + "fingertip_pos", + "fingertip_quat", + "ee_linvel", + "ee_angvel", + "fingertip_goal_pos", + "fingertip_goal_quat", + "held_pos", + "held_quat", + "delta_pos", + ] + + task_name: str = "extraction" # peg_insertion, gear_meshing, nut_threading + tasks: dict = {"extraction": Extraction()} + obs_rand: ObsRandCfg = ObsRandCfg() + ctrl: CtrlCfg = CtrlCfg() + + # episode_length_s = 10.0 # Probably need to override. + episode_length_s = 5.0 + sim: SimulationCfg = SimulationCfg( + device="cuda:0", + dt=1 / 120, + gravity=(0.0, 0.0, -9.81), + physx=PhysxCfg( + solver_type=1, + max_position_iteration_count=192, # Important to avoid interpenetration. + max_velocity_iteration_count=1, + bounce_threshold_velocity=0.2, + friction_offset_threshold=0.01, + friction_correlation_distance=0.00625, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + gpu_max_num_partitions=1, # Important for stable simulation. + ), + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + ) + + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0) + + robot = ArticulationCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ASSET_DIR}/franka_mimic.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.00871, + "panda_joint2": -0.10368, + "panda_joint3": -0.00794, + "panda_joint4": -1.49139, + "panda_joint5": -0.00083, + "panda_joint6": 1.38774, + "panda_joint7": 0.0, + "panda_finger_joint2": 0.04, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + actuators={ + "panda_arm1": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=87, + velocity_limit=124.6, + ), + "panda_arm2": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=12, + velocity_limit=149.5, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint[1-2]"], + effort_limit=40.0, + velocity_limit=0.04, + stiffness=7500.0, + damping=173.0, + friction=0.1, + armature=0.0, + ), + }, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py new file mode 100644 index 00000000000..0782403fbaf --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py @@ -0,0 +1,225 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +ASSET_DIR = f"{ISAACLAB_NUCLEUS_DIR}/AutoMate" + +OBS_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, +} + +STATE_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, + "joint_pos": 7, + "held_pos": 3, + "held_pos_rel_fixed": 3, + "held_quat": 4, + "fixed_pos": 3, + "fixed_quat": 4, + "task_prop_gains": 6, + "ema_factor": 1, + "pos_threshold": 3, + "rot_threshold": 3, +} + + +@configclass +class FixedAssetCfg: + usd_path: str = "" + diameter: float = 0.0 + height: float = 0.0 + base_height: float = 0.0 # Used to compute held asset CoM. + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class HeldAssetCfg: + usd_path: str = "" + diameter: float = 0.0 # Used for gripper width. + height: float = 0.0 + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class RobotCfg: + robot_usd: str = "" + franka_fingerpad_length: float = 0.017608 + friction: float = 0.75 + + +@configclass +class DisassemblyTask: + robot_cfg: RobotCfg = RobotCfg() + name: str = "" + duration_s = 5.0 + + fixed_asset_cfg: FixedAssetCfg = FixedAssetCfg() + held_asset_cfg: HeldAssetCfg = HeldAssetCfg() + asset_size: float = 0.0 + + # palm_to_finger_dist: float = 0.1034 + palm_to_finger_dist: float = 0.1134 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.015] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0, 2.356] + hand_init_orn_noise: list = [0.0, 0.0, 1.57] + + # Action + unidirectional_rot: bool = False + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 10.0 + + num_point_robot_traj: int = 10 # number of waypoints included in the end-effector trajectory + + +@configclass +class Peg8mm(HeldAssetCfg): + usd_path = "plug.usd" + obj_path = "plug.obj" + diameter = 0.007986 + height = 0.050 + mass = 0.019 + + +@configclass +class Hole8mm(FixedAssetCfg): + usd_path = "socket.usd" + obj_path = "socket.obj" + diameter = 0.0081 + height = 0.050896 + base_height = 0.0 + + +@configclass +class Extraction(DisassemblyTask): + name = "extraction" + + assembly_id = "00731" + assembly_dir = f"{ASSET_DIR}/{assembly_id}/" + disassembly_dir = "disassembly_dir" + num_log_traj = 100 + + fixed_asset_cfg = Hole8mm() + held_asset_cfg = Peg8mm() + asset_size = 8.0 + duration_s = 10.0 + + plug_grasp_json = f"{ASSET_DIR}/plug_grasps.json" + disassembly_dist_json = f"{ASSET_DIR}/disassembly_dist.json" + + move_gripper_sim_steps = 64 + disassemble_sim_steps = 64 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.047] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0.0, 0.0] + hand_init_orn_noise: list = [0.0, 0.0, 0.785] + hand_width_max: float = 0.080 # maximum opening width of gripper + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 10.0 + fixed_asset_z_offset: float = 0.1435 + + fingertip_centered_pos_initial: list = [ + 0.0, + 0.0, + 0.2, + ] # initial position of midpoint between fingertips above table + fingertip_centered_rot_initial: list = [3.141593, 0.0, 0.0] # initial rotation of fingertips (Euler) + gripper_rand_pos_noise: list = [0.05, 0.05, 0.05] + gripper_rand_rot_noise: list = [0.174533, 0.174533, 0.174533] # +-10 deg for roll/pitch/yaw + gripper_rand_z_offset: float = 0.05 + + fixed_asset: ArticulationCfg = ArticulationCfg( + # fixed_asset: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/FixedAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{assembly_dir}{fixed_asset_cfg.usd_path}", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + fix_root_link=True, # add this so the fixed asset is set to have a fixed base + ), + mass_props=sim_utils.MassPropertiesCfg(mass=fixed_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + # init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.6, 0.0, 0.05), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={}, + joint_vel={}, + ), + actuators={}, + ) + # held_asset: ArticulationCfg = ArticulationCfg( + held_asset: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/HeldAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{assembly_dir}{held_asset_cfg.usd_path}", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=held_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + # init_state=ArticulationCfg.InitialStateCfg( + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), + rot=(1.0, 0.0, 0.0, 0.0), + # joint_pos={}, + # joint_vel={} + ), + # actuators={} + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/factory_control.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/factory_control.py new file mode 100644 index 00000000000..a88f190ca00 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/factory_control.py @@ -0,0 +1,201 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Factory: control module. + +Imported by base, environment, and task classes. Not directly executed. +""" + +import math +import torch + +import isaacsim.core.utils.torch as torch_utils + +from isaaclab.utils.math import axis_angle_from_quat + + +def compute_dof_torque( + cfg, + dof_pos, + dof_vel, + fingertip_midpoint_pos, + fingertip_midpoint_quat, + fingertip_midpoint_linvel, + fingertip_midpoint_angvel, + jacobian, + arm_mass_matrix, + ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat, + task_prop_gains, + task_deriv_gains, + device, +): + """Compute Franka DOF torque to move fingertips towards target pose.""" + # References: + # 1) https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf + # 2) Modern Robotics + + num_envs = cfg.scene.num_envs + dof_torque = torch.zeros((num_envs, dof_pos.shape[1]), device=device) + task_wrench = torch.zeros((num_envs, 6), device=device) + + pos_error, axis_angle_error = get_pose_error( + fingertip_midpoint_pos=fingertip_midpoint_pos, + fingertip_midpoint_quat=fingertip_midpoint_quat, + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + delta_fingertip_pose = torch.cat((pos_error, axis_angle_error), dim=1) + + # Set tau = k_p * task_pos_error - k_d * task_vel_error (building towards eq. 3.96-3.98) + task_wrench_motion = _apply_task_space_gains( + delta_fingertip_pose=delta_fingertip_pose, + fingertip_midpoint_linvel=fingertip_midpoint_linvel, + fingertip_midpoint_angvel=fingertip_midpoint_angvel, + task_prop_gains=task_prop_gains, + task_deriv_gains=task_deriv_gains, + ) + task_wrench += task_wrench_motion + + # Set tau = J^T * tau, i.e., map tau into joint space as desired + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + dof_torque[:, 0:7] = (jacobian_T @ task_wrench.unsqueeze(-1)).squeeze(-1) + + # adapted from https://gitlab-master.nvidia.com/carbon-gym/carbgym/-/blob/b4bbc66f4e31b1a1bee61dbaafc0766bbfbf0f58/python/examples/franka_cube_ik_osc.py#L70-78 + # roboticsproceedings.org/rss07/p31.pdf + + # useful tensors + arm_mass_matrix_inv = torch.inverse(arm_mass_matrix) + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + arm_mass_matrix_task = torch.inverse( + jacobian @ torch.inverse(arm_mass_matrix) @ jacobian_T + ) # ETH eq. 3.86; geometric Jacobian is assumed + j_eef_inv = arm_mass_matrix_task @ jacobian @ arm_mass_matrix_inv + default_dof_pos_tensor = torch.tensor(cfg.ctrl.default_dof_pos_tensor, device=device).repeat((num_envs, 1)) + # nullspace computation + distance_to_default_dof_pos = default_dof_pos_tensor - dof_pos[:, :7] + distance_to_default_dof_pos = (distance_to_default_dof_pos + math.pi) % ( + 2 * math.pi + ) - math.pi # normalize to [-pi, pi] + u_null = cfg.ctrl.kd_null * -dof_vel[:, :7] + cfg.ctrl.kp_null * distance_to_default_dof_pos + u_null = arm_mass_matrix @ u_null.unsqueeze(-1) + torque_null = (torch.eye(7, device=device).unsqueeze(0) - torch.transpose(jacobian, 1, 2) @ j_eef_inv) @ u_null + dof_torque[:, 0:7] += torque_null.squeeze(-1) + + # TODO: Verify it's okay to no longer do gripper control here. + dof_torque = torch.clamp(dof_torque, min=-100.0, max=100.0) + return dof_torque, task_wrench + + +def get_pose_error( + fingertip_midpoint_pos, + fingertip_midpoint_quat, + ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat, + jacobian_type, + rot_error_type, +): + """Compute task-space error between target Franka fingertip pose and current pose.""" + # Reference: https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf + + # Compute pos error + pos_error = ctrl_target_fingertip_midpoint_pos - fingertip_midpoint_pos + + # Compute rot error + if jacobian_type == "geometric": # See example 2.9.8; note use of J_g and transformation between rotation vectors + # Compute quat error (i.e., difference quat) + # Reference: https://personal.utdallas.edu/~sxb027100/dock/quat.html + + # Check for shortest path using quaternion dot product. + quat_dot = (ctrl_target_fingertip_midpoint_quat * fingertip_midpoint_quat).sum(dim=1, keepdim=True) + ctrl_target_fingertip_midpoint_quat = torch.where( + quat_dot.expand(-1, 4) >= 0, ctrl_target_fingertip_midpoint_quat, -ctrl_target_fingertip_midpoint_quat + ) + + fingertip_midpoint_quat_norm = torch_utils.quat_mul( + fingertip_midpoint_quat, torch_utils.quat_conjugate(fingertip_midpoint_quat) + )[ + :, 0 + ] # scalar component + fingertip_midpoint_quat_inv = torch_utils.quat_conjugate( + fingertip_midpoint_quat + ) / fingertip_midpoint_quat_norm.unsqueeze(-1) + quat_error = torch_utils.quat_mul(ctrl_target_fingertip_midpoint_quat, fingertip_midpoint_quat_inv) + + # Convert to axis-angle error + axis_angle_error = axis_angle_from_quat(quat_error) + + if rot_error_type == "quat": + return pos_error, quat_error + elif rot_error_type == "axis_angle": + return pos_error, axis_angle_error + + +def _get_delta_dof_pos(delta_pose, ik_method, jacobian, device): + """Get delta Franka DOF position from delta pose using specified IK method.""" + # References: + # 1) https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf + # 2) https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf (p. 47) + + if ik_method == "pinv": # Jacobian pseudoinverse + k_val = 1.0 + jacobian_pinv = torch.linalg.pinv(jacobian) + delta_dof_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "trans": # Jacobian transpose + k_val = 1.0 + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + delta_dof_pos = k_val * jacobian_T @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "dls": # damped least squares (Levenberg-Marquardt) + lambda_val = 0.1 # 0.1 + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=device) + delta_dof_pos = jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "svd": # adaptive SVD + k_val = 1.0 + U, S, Vh = torch.linalg.svd(jacobian) + S_inv = 1.0 / S + min_singular_value = 1.0e-5 + S_inv = torch.where(S > min_singular_value, S_inv, torch.zeros_like(S_inv)) + jacobian_pinv = ( + torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] @ torch.diag_embed(S_inv) @ torch.transpose(U, dim0=1, dim1=2) + ) + delta_dof_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + return delta_dof_pos + + +def _apply_task_space_gains( + delta_fingertip_pose, fingertip_midpoint_linvel, fingertip_midpoint_angvel, task_prop_gains, task_deriv_gains +): + """Interpret PD gains as task-space gains. Apply to task-space error.""" + + task_wrench = torch.zeros_like(delta_fingertip_pose) + + # Apply gains to lin error components + lin_error = delta_fingertip_pose[:, 0:3] + task_wrench[:, 0:3] = task_prop_gains[:, 0:3] * lin_error + task_deriv_gains[:, 0:3] * ( + 0.0 - fingertip_midpoint_linvel + ) + + # Apply gains to rot error components + rot_error = delta_fingertip_pose[:, 3:6] + task_wrench[:, 3:6] = task_prop_gains[:, 3:6] * rot_error + task_deriv_gains[:, 3:6] * ( + 0.0 - fingertip_midpoint_angvel + ) + return task_wrench diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/industreal_algo_utils.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/industreal_algo_utils.py new file mode 100644 index 00000000000..317915b81df --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/industreal_algo_utils.py @@ -0,0 +1,371 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2023, NVIDIA Corporation +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +"""IndustReal: algorithms module. + +Contains functions that implement Simulation-Aware Policy Update (SAPU), SDF-Based Reward, and Sampling-Based Curriculum (SBC). + +Not intended to be executed as a standalone script. +""" + +import numpy as np +import os + +# from pysdf import SDF +import torch +import trimesh +from trimesh.exchange.load import load + +# from urdfpy import URDF +import warp as wp + +from isaaclab.utils.assets import retrieve_file_path + +""" +Simulation-Aware Policy Update (SAPU) +""" + + +def load_asset_mesh_in_warp(held_asset_obj, fixed_asset_obj, num_samples, device): + """Create mesh objects in Warp for all environments.""" + retrieve_file_path(held_asset_obj, download_dir="./") + plug_trimesh = load(os.path.basename(held_asset_obj)) + # plug_trimesh = load(held_asset_obj) + retrieve_file_path(fixed_asset_obj, download_dir="./") + socket_trimesh = load(os.path.basename(fixed_asset_obj)) + # socket_trimesh = load(fixed_asset_obj) + + plug_wp_mesh = wp.Mesh( + points=wp.array(plug_trimesh.vertices, dtype=wp.vec3, device=device), + indices=wp.array(plug_trimesh.faces.flatten(), dtype=wp.int32, device=device), + ) + + # Sample points on surface of mesh + sampled_points, _ = trimesh.sample.sample_surface_even(plug_trimesh, num_samples) + wp_mesh_sampled_points = wp.array(sampled_points, dtype=wp.vec3, device=device) + + socket_wp_mesh = wp.Mesh( + points=wp.array(socket_trimesh.vertices, dtype=wp.vec3, device=device), + indices=wp.array(socket_trimesh.faces.flatten(), dtype=wp.int32, device=device), + ) + + return plug_wp_mesh, wp_mesh_sampled_points, socket_wp_mesh + + +""" +SDF-Based Reward +""" + + +def get_sdf_reward( + wp_plug_mesh, + wp_plug_mesh_sampled_points, + plug_pos, + plug_quat, + socket_pos, + socket_quat, + wp_device, + device, +): + """Calculate SDF-based reward.""" + + num_envs = len(plug_pos) + sdf_reward = torch.zeros((num_envs,), dtype=torch.float32, device=device) + + for i in range(num_envs): + + # Create copy of plug mesh + mesh_points = wp.clone(wp_plug_mesh.points) + mesh_indices = wp.clone(wp_plug_mesh.indices) + mesh_copy = wp.Mesh(points=mesh_points, indices=mesh_indices) + + # Transform plug mesh from current pose to goal pose + # NOTE: In source OBJ files, when plug and socket are assembled, + # their poses are identical + goal_transform = wp.transform(socket_pos[i], socket_quat[i]) + wp.launch( + kernel=transform_points, + dim=len(mesh_copy.points), + inputs=[mesh_copy.points, mesh_copy.points, goal_transform], + device=wp_device, + ) + + # Rebuild BVH (see https://nvidia.github.io/warp/_build/html/modules/runtime.html#meshes) + mesh_copy.refit() + + # Create copy of sampled points + sampled_points = wp.clone(wp_plug_mesh_sampled_points) + + # Transform sampled points from original plug pose to current plug pose + curr_transform = wp.transform(plug_pos[i], plug_quat[i]) + wp.launch( + kernel=transform_points, + dim=len(sampled_points), + inputs=[sampled_points, sampled_points, curr_transform], + device=wp_device, + ) + + # Get SDF values at transformed points + sdf_dist = wp.zeros((len(sampled_points),), dtype=wp.float32, device=wp_device) + wp.launch( + kernel=get_batch_sdf, + dim=len(sampled_points), + inputs=[mesh_copy.id, sampled_points, sdf_dist], + device=wp_device, + ) + sdf_dist = wp.to_torch(sdf_dist) + + # Clamp values outside isosurface and take absolute value + sdf_dist = torch.where(sdf_dist < 0.0, 0.0, sdf_dist) + + sdf_reward[i] = torch.mean(sdf_dist) + + sdf_reward = -torch.log(sdf_reward) + + return sdf_reward + + +""" +Sampling-Based Curriculum (SBC) +""" + + +def get_curriculum_reward_scale(cfg_task, curr_max_disp): + """Compute reward scale for SBC.""" + + # Compute difference between max downward displacement at beginning of training (easiest condition) + # and current max downward displacement (based on current curriculum stage) + # NOTE: This number increases as curriculum gets harder + curr_stage_diff = cfg_task.curriculum_height_bound[1] - curr_max_disp + + # Compute difference between max downward displacement at beginning of training (easiest condition) + # and min downward displacement (hardest condition) + final_stage_diff = cfg_task.curriculum_height_bound[1] - cfg_task.curriculum_height_bound[0] + + # Compute reward scale + reward_scale = curr_stage_diff / final_stage_diff + 1.0 + + return reward_scale + + +def get_new_max_disp(curr_success, cfg_task, curr_max_disp): + """Update max downward displacement of plug at beginning of episode, based on success rate.""" + + if curr_success > cfg_task.curriculum_success_thresh: + # If success rate is above threshold, reduce max downward displacement until min value + # NOTE: height_step[0] is negative + new_max_disp = max( + curr_max_disp + cfg_task.curriculum_height_step[0], + cfg_task.curriculum_height_bound[0], + ) + + elif curr_success < cfg_task.curriculum_failure_thresh: + # If success rate is below threshold, increase max downward displacement until max value + # NOTE: height_step[1] is positive + new_max_disp = min( + curr_max_disp + cfg_task.curriculum_height_step[1], + cfg_task.curriculum_height_bound[1], + ) + + else: + # Maintain current max downward displacement + new_max_disp = curr_max_disp + + return new_max_disp + + +""" +Bonus and Success Checking +""" + + +def get_keypoint_offsets(num_keypoints, device): + """Get uniformly-spaced keypoints along a line of unit length, centered at 0.""" + + keypoint_offsets = torch.zeros((num_keypoints, 3), device=device) + keypoint_offsets[:, -1] = torch.linspace(0.0, 1.0, num_keypoints, device=device) - 0.5 + + return keypoint_offsets + + +def check_plug_close_to_socket(keypoints_plug, keypoints_socket, dist_threshold, progress_buf): + """Check if plug is close to socket.""" + + # Compute keypoint distance between plug and socket + keypoint_dist = torch.norm(keypoints_socket - keypoints_plug, p=2, dim=-1) + + # Check if keypoint distance is below threshold + is_plug_close_to_socket = torch.where( + torch.sum(keypoint_dist, dim=-1) < dist_threshold, + torch.ones_like(progress_buf), + torch.zeros_like(progress_buf), + ) + + return is_plug_close_to_socket + + +def check_plug_inserted_in_socket( + plug_pos, socket_pos, keypoints_plug, keypoints_socket, success_height_thresh, close_error_thresh, progress_buf +): + """Check if plug is inserted in socket.""" + + # Check if plug is within threshold distance of assembled state + is_plug_below_insertion_height = plug_pos[:, 2] < socket_pos[:, 2] + success_height_thresh + + # Check if plug is close to socket + # NOTE: This check addresses edge case where plug is within threshold distance of + # assembled state, but plug is outside socket + is_plug_close_to_socket = check_plug_close_to_socket( + keypoints_plug=keypoints_plug, + keypoints_socket=keypoints_socket, + dist_threshold=close_error_thresh, + progress_buf=progress_buf, + ) + + # Combine both checks + is_plug_inserted_in_socket = torch.logical_and(is_plug_below_insertion_height, is_plug_close_to_socket) + + return is_plug_inserted_in_socket + + +def get_engagement_reward_scale(plug_pos, socket_pos, is_plug_engaged_w_socket, success_height_thresh, device): + """Compute scale on reward. If plug is not engaged with socket, scale is zero. + If plug is engaged, scale is proportional to distance between plug and bottom of socket.""" + + # Set default value of scale to zero + num_envs = len(plug_pos) + reward_scale = torch.zeros((num_envs,), dtype=torch.float32, device=device) + + # For envs in which plug and socket are engaged, compute positive scale + engaged_idx = np.argwhere(is_plug_engaged_w_socket.cpu().numpy().copy()).squeeze() + height_dist = plug_pos[engaged_idx, 2] - socket_pos[engaged_idx, 2] + # NOTE: Edge case: if success_height_thresh is greater than 0.1, + # denominator could be negative + reward_scale[engaged_idx] = 1.0 / ((height_dist - success_height_thresh) + 0.1) + + return reward_scale + + +""" +Warp Functions +""" + + +@wp.func +def mesh_sdf(mesh: wp.uint64, point: wp.vec3, max_dist: float): + face_index = int(0) + face_u = float(0.0) + face_v = float(0.0) + sign = float(0.0) + res = wp.mesh_query_point(mesh, point, max_dist, sign, face_index, face_u, face_v) + if res: + closest = wp.mesh_eval_position(mesh, face_index, face_u, face_v) + return wp.length(point - closest) * sign + return max_dist + + +""" +Warp Kernels +""" + + +@wp.kernel +def get_batch_sdf( + mesh: wp.uint64, + queries: wp.array(dtype=wp.vec3), + sdf_dist: wp.array(dtype=wp.float32), +): + tid = wp.tid() + + q = queries[tid] # query point + max_dist = 1.5 # max distance on mesh from query point + # max_dist = 0.0 + + # sdf_dist[tid] = wp.mesh_query_point_sign_normal(mesh, q, max_dist) + sdf_dist[tid] = mesh_sdf(mesh, q, max_dist) + + +# Transform points from source coordinate frame to destination coordinate frame +@wp.kernel +def transform_points(src: wp.array(dtype=wp.vec3), dest: wp.array(dtype=wp.vec3), xform: wp.transform): + tid = wp.tid() + + p = src[tid] + m = wp.transform_point(xform, p) + + dest[tid] = m + + +# Return interpenetration distances between query points (e.g., plug vertices in current pose) +# and mesh surfaces (e.g., of socket mesh in current pose) +@wp.kernel +def get_interpen_dist( + queries: wp.array(dtype=wp.vec3), + mesh: wp.uint64, + interpen_dists: wp.array(dtype=wp.float32), +): + tid = wp.tid() + + # Declare arguments to wp.mesh_query_point() that will not be modified + q = queries[tid] # query point + max_dist = 1.5 # max distance on mesh from query point + + # Declare arguments to wp.mesh_query_point() that will be modified + sign = float( + 0.0 + ) # -1 if query point inside mesh; 0 if on mesh; +1 if outside mesh (NOTE: Mesh must be watertight!) + face_idx = int(0) # index of closest face + face_u = float(0.0) # barycentric u-coordinate of closest point + face_v = float(0.0) # barycentric v-coordinate of closest point + + # Get closest point on mesh to query point + closest_mesh_point_exists = wp.mesh_query_point(mesh, q, max_dist, sign, face_idx, face_u, face_v) + + # If point exists within max_dist + if closest_mesh_point_exists: + # Get 3D position of point on mesh given face index and barycentric coordinates + p = wp.mesh_eval_position(mesh, face_idx, face_u, face_v) + + # Get signed distance between query point and mesh point + delta = q - p + signed_dist = sign * wp.length(delta) + + # If signed distance is negative + if signed_dist < 0.0: + # Store interpenetration distance + interpen_dists[tid] = signed_dist diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_disassembly_w_id.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_disassembly_w_id.py new file mode 100644 index 00000000000..b5e49b259ed --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_disassembly_w_id.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import argparse +import os +import re +import subprocess + + +def update_task_param(task_cfg, assembly_id, disassembly_dir): + # Read the file lines. + with open(task_cfg) as f: + lines = f.readlines() + + updated_lines = [] + + # Regex patterns to capture the assignment lines + assembly_pattern = re.compile(r"^(.*assembly_id\s*=\s*).*$") + disassembly_dir_pattern = re.compile(r"^(.*disassembly_dir\s*=\s*).*$") + + for line in lines: + if "assembly_id =" in line: + line = assembly_pattern.sub(rf"\1'{assembly_id}'", line) + elif "disassembly_dir = " in line: + line = disassembly_dir_pattern.sub(rf"\1'{disassembly_dir}'", line) + + updated_lines.append(line) + + # Write the modified lines back to the file. + with open(task_cfg, "w") as f: + f.writelines(updated_lines) + + +def main(): + parser = argparse.ArgumentParser(description="Update assembly_id and run training script.") + parser.add_argument( + "--disassembly_dir", + type=str, + help="Path to the directory containing output disassembly trajectories.", + default="disassembly_dir", + ) + parser.add_argument( + "--cfg_path", + type=str, + help="Path to the file containing assembly_id.", + default="source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py", + ) + parser.add_argument("--assembly_id", type=str, default="00731", help="New assembly ID to set.") + parser.add_argument("--num_envs", type=int, default=128, help="Number of parallel environment.") + parser.add_argument("--seed", type=int, default=-1, help="Random seed.") + parser.add_argument("--headless", action="store_true", help="Run in headless mode.") + args = parser.parse_args() + + os.makedirs(args.disassembly_dir, exist_ok=True) + + update_task_param( + args.cfg_path, + args.assembly_id, + args.disassembly_dir, + ) + + bash_command = ( + "./isaaclab.sh -p scripts/reinforcement_learning/rl_games/train.py --task=Isaac-AutoMate-Disassembly-Direct-v0" + ) + + bash_command += f" --num_envs={str(args.num_envs)}" + bash_command += f" --seed={str(args.seed)}" + + if args.headless: + bash_command += " --headless" + + # Run the bash command + subprocess.run(bash_command, shell=True, check=True) + + +if __name__ == "__main__": + main() diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_w_id.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_w_id.py new file mode 100644 index 00000000000..51f21e91673 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/run_w_id.py @@ -0,0 +1,91 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import argparse +import re +import subprocess + + +def update_task_param(task_cfg, assembly_id, if_sbc, if_log_eval): + # Read the file lines. + with open(task_cfg) as f: + lines = f.readlines() + + updated_lines = [] + + # Regex patterns to capture the assignment lines + assembly_pattern = re.compile(r"^(.*assembly_id\s*=\s*).*$") + if_sbc_pattern = re.compile(r"^(.*if_sbc\s*:\s*bool\s*=\s*).*$") + if_log_eval_pattern = re.compile(r"^(.*if_logging_eval\s*:\s*bool\s*=\s*).*$") + eval_file_pattern = re.compile(r"^(.*eval_filename\s*:\s*str\s*=\s*).*$") + + for line in lines: + if "assembly_id =" in line: + line = assembly_pattern.sub(rf"\1'{assembly_id}'", line) + elif "if_sbc: bool =" in line: + line = if_sbc_pattern.sub(rf"\1{str(if_sbc)}", line) + elif "if_logging_eval: bool =" in line: + line = if_log_eval_pattern.sub(rf"\1{str(if_log_eval)}", line) + elif "eval_filename: str = " in line: + line = eval_file_pattern.sub(r"\1'{}'".format(f"evaluation_{assembly_id}.h5"), line) + + updated_lines.append(line) + + # Write the modified lines back to the file. + with open(task_cfg, "w") as f: + f.writelines(updated_lines) + + +def main(): + parser = argparse.ArgumentParser(description="Update assembly_id and run training script.") + parser.add_argument( + "--cfg_path", + type=str, + help="Path to the file containing assembly_id.", + default="source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py", + ) + parser.add_argument("--assembly_id", type=str, help="New assembly ID to set.") + parser.add_argument("--checkpoint", type=str, help="Checkpoint path.") + parser.add_argument("--num_envs", type=int, default=128, help="Number of parallel environment.") + parser.add_argument("--seed", type=int, default=-1, help="Random seed.") + parser.add_argument("--train", action="store_true", help="Run training mode.") + parser.add_argument("--log_eval", action="store_true", help="Log evaluation results.") + parser.add_argument("--headless", action="store_true", help="Run in headless mode.") + args = parser.parse_args() + + update_task_param(args.cfg_path, args.assembly_id, args.train, args.log_eval) + + bash_command = None + if args.train: + bash_command = ( + "./isaaclab.sh -p scripts/reinforcement_learning/rl_games/train.py --task=Isaac-AutoMate-Assembly-Direct-v0" + ) + bash_command += f" --seed={str(args.seed)}" + else: + if not args.checkpoint: + raise ValueError("No checkpoint provided for evaluation.") + bash_command = ( + "./isaaclab.sh -p scripts/reinforcement_learning/rl_games/play.py --task=Isaac-AutoMate-Assembly-Direct-v0" + ) + + bash_command += f" --num_envs={str(args.num_envs)}" + + if args.checkpoint: + bash_command += f" --checkpoint={args.checkpoint}" + + if args.headless: + bash_command += " --headless" + + # Run the bash command + subprocess.run(bash_command, shell=True, check=True) + + +if __name__ == "__main__": + main() diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/soft_dtw_cuda.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/soft_dtw_cuda.py new file mode 100644 index 00000000000..6881917be18 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/automate/soft_dtw_cuda.py @@ -0,0 +1,455 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# MIT License +# +# Copyright (c) 2020 Mehran Maghoumi +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# ---------------------------------------------------------------------------------------------------------------------- + +import math +import numpy as np +import torch +import torch.cuda +from torch.autograd import Function + +from numba import cuda, jit, prange + + +# ---------------------------------------------------------------------------------------------------------------------- +@cuda.jit +def compute_softdtw_cuda(D, gamma, bandwidth, max_i, max_j, n_passes, R): + """ + :param seq_len: The length of the sequence (both inputs are assumed to be of the same size) + :param n_passes: 2 * seq_len - 1 (The number of anti-diagonals) + """ + # Each block processes one pair of examples + b = cuda.blockIdx.x + # We have as many threads as seq_len, because the most number of threads we need + # is equal to the number of elements on the largest anti-diagonal + tid = cuda.threadIdx.x + + inv_gamma = 1.0 / gamma + + # Go over each anti-diagonal. Only process threads that fall on the current on the anti-diagonal + for p in range(n_passes): + + # The index is actually 'p - tid' but need to force it in-bounds + J = max(0, min(p - tid, max_j - 1)) + + # For simplicity, we define i, j which start from 1 (offset from I, J) + i = tid + 1 + j = J + 1 + + # Only compute if element[i, j] is on the current anti-diagonal, and also is within bounds + if tid + J == p and (tid < max_i and J < max_j): + # Don't compute if outside bandwidth + if not (abs(i - j) > bandwidth > 0): + r0 = -R[b, i - 1, j - 1] * inv_gamma + r1 = -R[b, i - 1, j] * inv_gamma + r2 = -R[b, i, j - 1] * inv_gamma + rmax = max(max(r0, r1), r2) + rsum = math.exp(r0 - rmax) + math.exp(r1 - rmax) + math.exp(r2 - rmax) + softmin = -gamma * (math.log(rsum) + rmax) + R[b, i, j] = D[b, i - 1, j - 1] + softmin + + # Wait for other threads in this block + cuda.syncthreads() + + +# ---------------------------------------------------------------------------------------------------------------------- +@cuda.jit +def compute_softdtw_backward_cuda(D, R, inv_gamma, bandwidth, max_i, max_j, n_passes, E): + k = cuda.blockIdx.x + tid = cuda.threadIdx.x + + # Indexing logic is the same as above, however, the anti-diagonal needs to + # progress backwards + + for p in range(n_passes): + # Reverse the order to make the loop go backward + rev_p = n_passes - p - 1 + + # convert tid to I, J, then i, j + J = max(0, min(rev_p - tid, max_j - 1)) + + i = tid + 1 + j = J + 1 + + # Only compute if element[i, j] is on the current anti-diagonal, and also is within bounds + if tid + J == rev_p and (tid < max_i and J < max_j): + + if math.isinf(R[k, i, j]): + R[k, i, j] = -math.inf + + # Don't compute if outside bandwidth + if not (abs(i - j) > bandwidth > 0): + a = math.exp((R[k, i + 1, j] - R[k, i, j] - D[k, i + 1, j]) * inv_gamma) + b = math.exp((R[k, i, j + 1] - R[k, i, j] - D[k, i, j + 1]) * inv_gamma) + c = math.exp((R[k, i + 1, j + 1] - R[k, i, j] - D[k, i + 1, j + 1]) * inv_gamma) + E[k, i, j] = E[k, i + 1, j] * a + E[k, i, j + 1] * b + E[k, i + 1, j + 1] * c + + # Wait for other threads in this block + cuda.syncthreads() + + +# ---------------------------------------------------------------------------------------------------------------------- +class _SoftDTWCUDA(Function): + """ + CUDA implementation is inspired by the diagonal one proposed in https://ieeexplore.ieee.org/document/8400444: + "Developing a pattern discovery method in time series data and its GPU acceleration" + """ + + @staticmethod + def forward(ctx, D, gamma, bandwidth): + dev = D.device + dtype = D.dtype + gamma = torch.cuda.FloatTensor([gamma]) + bandwidth = torch.cuda.FloatTensor([bandwidth]) + + B = D.shape[0] + N = D.shape[1] + M = D.shape[2] + threads_per_block = max(N, M) + n_passes = 2 * threads_per_block - 1 + + # Prepare the output array + R = torch.ones((B, N + 2, M + 2), device=dev, dtype=dtype) * math.inf + R[:, 0, 0] = 0 + + # Run the CUDA kernel. + # Set CUDA's grid size to be equal to the batch size (every CUDA block processes one sample pair) + # Set the CUDA block size to be equal to the length of the longer sequence (equal to the size of the largest diagonal) + compute_softdtw_cuda[B, threads_per_block]( + cuda.as_cuda_array(D.detach()), gamma.item(), bandwidth.item(), N, M, n_passes, cuda.as_cuda_array(R) + ) + ctx.save_for_backward(D, R.clone(), gamma, bandwidth) + return R[:, -2, -2] + + @staticmethod + def backward(ctx, grad_output): + dev = grad_output.device + dtype = grad_output.dtype + D, R, gamma, bandwidth = ctx.saved_tensors + + B = D.shape[0] + N = D.shape[1] + M = D.shape[2] + threads_per_block = max(N, M) + n_passes = 2 * threads_per_block - 1 + + D_ = torch.zeros((B, N + 2, M + 2), dtype=dtype, device=dev) + D_[:, 1 : N + 1, 1 : M + 1] = D + + R[:, :, -1] = -math.inf + R[:, -1, :] = -math.inf + R[:, -1, -1] = R[:, -2, -2] + + E = torch.zeros((B, N + 2, M + 2), dtype=dtype, device=dev) + E[:, -1, -1] = 1 + + # Grid and block sizes are set same as done above for the forward() call + compute_softdtw_backward_cuda[B, threads_per_block]( + cuda.as_cuda_array(D_), + cuda.as_cuda_array(R), + 1.0 / gamma.item(), + bandwidth.item(), + N, + M, + n_passes, + cuda.as_cuda_array(E), + ) + E = E[:, 1 : N + 1, 1 : M + 1] + return grad_output.view(-1, 1, 1).expand_as(E) * E, None, None + + +# ---------------------------------------------------------------------------------------------------------------------- +# +# The following is the CPU implementation based on https://github.com/Sleepwalking/pytorch-softdtw +# Credit goes to Kanru Hua. +# I've added support for batching and pruning. +# +# ---------------------------------------------------------------------------------------------------------------------- +@jit(nopython=True, parallel=True) +def compute_softdtw(D, gamma, bandwidth): + B = D.shape[0] + N = D.shape[1] + M = D.shape[2] + R = np.ones((B, N + 2, M + 2)) * np.inf + R[:, 0, 0] = 0 + for b in prange(B): + for j in range(1, M + 1): + for i in range(1, N + 1): + + # Check the pruning condition + if 0 < bandwidth < np.abs(i - j): + continue + + r0 = -R[b, i - 1, j - 1] / gamma + r1 = -R[b, i - 1, j] / gamma + r2 = -R[b, i, j - 1] / gamma + rmax = max(max(r0, r1), r2) + rsum = np.exp(r0 - rmax) + np.exp(r1 - rmax) + np.exp(r2 - rmax) + softmin = -gamma * (np.log(rsum) + rmax) + R[b, i, j] = D[b, i - 1, j - 1] + softmin + return R + + +# ---------------------------------------------------------------------------------------------------------------------- +@jit(nopython=True, parallel=True) +def compute_softdtw_backward(D_, R, gamma, bandwidth): + B = D_.shape[0] + N = D_.shape[1] + M = D_.shape[2] + D = np.zeros((B, N + 2, M + 2)) + E = np.zeros((B, N + 2, M + 2)) + D[:, 1 : N + 1, 1 : M + 1] = D_ + E[:, -1, -1] = 1 + R[:, :, -1] = -np.inf + R[:, -1, :] = -np.inf + R[:, -1, -1] = R[:, -2, -2] + for k in prange(B): + for j in range(M, 0, -1): + for i in range(N, 0, -1): + + if np.isinf(R[k, i, j]): + R[k, i, j] = -np.inf + + # Check the pruning condition + if 0 < bandwidth < np.abs(i - j): + continue + + a0 = (R[k, i + 1, j] - R[k, i, j] - D[k, i + 1, j]) / gamma + b0 = (R[k, i, j + 1] - R[k, i, j] - D[k, i, j + 1]) / gamma + c0 = (R[k, i + 1, j + 1] - R[k, i, j] - D[k, i + 1, j + 1]) / gamma + a = np.exp(a0) + b = np.exp(b0) + c = np.exp(c0) + E[k, i, j] = E[k, i + 1, j] * a + E[k, i, j + 1] * b + E[k, i + 1, j + 1] * c + return E[:, 1 : N + 1, 1 : M + 1] + + +# ---------------------------------------------------------------------------------------------------------------------- +class _SoftDTW(Function): + """ + CPU implementation based on https://github.com/Sleepwalking/pytorch-softdtw + """ + + @staticmethod + def forward(ctx, D, gamma, bandwidth): + dev = D.device + dtype = D.dtype + gamma = torch.Tensor([gamma]).to(dev).type(dtype) # dtype fixed + bandwidth = torch.Tensor([bandwidth]).to(dev).type(dtype) + D_ = D.detach().cpu().numpy() + g_ = gamma.item() + b_ = bandwidth.item() + R = torch.Tensor(compute_softdtw(D_, g_, b_)).to(dev).type(dtype) + ctx.save_for_backward(D, R, gamma, bandwidth) + return R[:, -2, -2] + + @staticmethod + def backward(ctx, grad_output): + dev = grad_output.device + dtype = grad_output.dtype + D, R, gamma, bandwidth = ctx.saved_tensors + D_ = D.detach().cpu().numpy() + R_ = R.detach().cpu().numpy() + g_ = gamma.item() + b_ = bandwidth.item() + E = torch.Tensor(compute_softdtw_backward(D_, R_, g_, b_)).to(dev).type(dtype) + return grad_output.view(-1, 1, 1).expand_as(E) * E, None, None + + +# ---------------------------------------------------------------------------------------------------------------------- +class SoftDTW(torch.nn.Module): + """ + The soft DTW implementation that optionally supports CUDA + """ + + def __init__(self, use_cuda, gamma=1.0, normalize=False, bandwidth=None, dist_func=None): + """ + Initializes a new instance using the supplied parameters + :param use_cuda: Flag indicating whether the CUDA implementation should be used + :param gamma: sDTW's gamma parameter + :param normalize: Flag indicating whether to perform normalization + (as discussed in https://github.com/mblondel/soft-dtw/issues/10#issuecomment-383564790) + :param bandwidth: Sakoe-Chiba bandwidth for pruning. Passing 'None' will disable pruning. + :param dist_func: Optional point-wise distance function to use. If 'None', then a default Euclidean distance function will be used. + """ + super().__init__() + self.normalize = normalize + self.gamma = gamma + self.bandwidth = 0 if bandwidth is None else float(bandwidth) + self.use_cuda = use_cuda + + # Set the distance function + if dist_func is not None: + self.dist_func = dist_func + else: + self.dist_func = SoftDTW._euclidean_dist_func + + def _get_func_dtw(self, x, y): + """ + Checks the inputs and selects the proper implementation to use. + """ + bx, lx, dx = x.shape + by, ly, dy = y.shape + # Make sure the dimensions match + assert bx == by # Equal batch sizes + assert dx == dy # Equal feature dimensions + + use_cuda = self.use_cuda + + if use_cuda and (lx > 1024 or ly > 1024): # We should be able to spawn enough threads in CUDA + print( + "SoftDTW: Cannot use CUDA because the sequence length > 1024 (the maximum block size supported by CUDA)" + ) + use_cuda = False + + # Finally, return the correct function + return _SoftDTWCUDA.apply if use_cuda else _SoftDTW.apply + + @staticmethod + def _euclidean_dist_func(x, y): + """ + Calculates the Euclidean distance between each element in x and y per timestep + """ + n = x.size(1) + m = y.size(1) + d = x.size(2) + x = x.unsqueeze(2).expand(-1, n, m, d) + y = y.unsqueeze(1).expand(-1, n, m, d) + return torch.pow(x - y, 2).sum(3) + + def forward(self, X, Y): + """ + Compute the soft-DTW value between X and Y + :param X: One batch of examples, batch_size x seq_len x dims + :param Y: The other batch of examples, batch_size x seq_len x dims + :return: The computed results + """ + + # Check the inputs and get the correct implementation + func_dtw = self._get_func_dtw(X, Y) + + if self.normalize: + # Stack everything up and run + x = torch.cat([X, X, Y]) + y = torch.cat([Y, X, Y]) + D = self.dist_func(x, y) + out = func_dtw(D, self.gamma, self.bandwidth) + out_xy, out_xx, out_yy = torch.split(out, X.shape[0]) + return out_xy - 1 / 2 * (out_xx + out_yy) + else: + D_xy = self.dist_func(X, Y) + return func_dtw(D_xy, self.gamma, self.bandwidth) + + +# ---------------------------------------------------------------------------------------------------------------------- +def timed_run(a, b, sdtw): + """ + Runs a and b through sdtw, and times the forward and backward passes. + Assumes that a requires gradients. + :return: timing, forward result, backward result + """ + + from timeit import default_timer as timer + + # Forward pass + start = timer() + forward = sdtw(a, b) + end = timer() + t = end - start + + grad_outputs = torch.ones_like(forward) + + # Backward + start = timer() + grads = torch.autograd.grad(forward, a, grad_outputs=grad_outputs)[0] + end = timer() + + # Total time + t += end - start + + return t, forward, grads + + +# ---------------------------------------------------------------------------------------------------------------------- +def profile(batch_size, seq_len_a, seq_len_b, dims, tol_backward): + sdtw = SoftDTW(False, gamma=1.0, normalize=False) + sdtw_cuda = SoftDTW(True, gamma=1.0, normalize=False) + n_iters = 6 + + print( + "Profiling forward() + backward() times for batch_size={}, seq_len_a={}, seq_len_b={}, dims={}...".format( + batch_size, seq_len_a, seq_len_b, dims + ) + ) + + times_cpu = [] + times_gpu = [] + + for i in range(n_iters): + a_cpu = torch.rand((batch_size, seq_len_a, dims), requires_grad=True) + b_cpu = torch.rand((batch_size, seq_len_b, dims)) + a_gpu = a_cpu.cuda() + b_gpu = b_cpu.cuda() + + # GPU + t_gpu, forward_gpu, backward_gpu = timed_run(a_gpu, b_gpu, sdtw_cuda) + + # CPU + t_cpu, forward_cpu, backward_cpu = timed_run(a_cpu, b_cpu, sdtw) + + # Verify the results + assert torch.allclose(forward_cpu, forward_gpu.cpu()) + assert torch.allclose(backward_cpu, backward_gpu.cpu(), atol=tol_backward) + + if ( + i > 0 + ): # Ignore the first time we run, in case this is a cold start (because timings are off at a cold start of the script) + times_cpu += [t_cpu] + times_gpu += [t_gpu] + + # Average and log + avg_cpu = np.mean(times_cpu) + avg_gpu = np.mean(times_gpu) + print(" CPU: ", avg_cpu) + print(" GPU: ", avg_gpu) + print(" Speedup: ", avg_cpu / avg_gpu) + print() + + +# ---------------------------------------------------------------------------------------------------------------------- +if __name__ == "__main__": + + torch.manual_seed(1234) + + profile(128, 17, 15, 2, tol_backward=1e-6) + profile(512, 64, 64, 2, tol_backward=1e-4) + profile(512, 256, 256, 2, tol_backward=1e-3) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/__init__.py new file mode 100644 index 00000000000..449dfebd55f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Inverted Double Pendulum on a Cart balancing environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Cart-Double-Pendulum-Direct-v0", + entry_point=f"{__name__}.cart_double_pendulum_env:CartDoublePendulumEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cart_double_pendulum_env:CartDoublePendulumEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "skrl_ippo_cfg_entry_point": f"{agents.__name__}:skrl_ippo_cfg.yaml", + "skrl_mappo_cfg_entry_point": f"{agents.__name__}:skrl_mappo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..6c2a3ad8473 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [32, 32] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cart_double_pendulum_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.1 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 150 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 16384 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml new file mode 100644 index 00000000000..bc0c5182179 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# IPPO agent configuration (field names are from IPPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/multi_agents/ippo.html +agent: + class: IPPO + rollouts: 16 + learning_epochs: 8 + mini_batches: 1 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cart_double_pendulum_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml new file mode 100644 index 00000000000..dcd794f57a5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml @@ -0,0 +1,82 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# MAPPO agent configuration (field names are from MAPPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/multi_agents/mappo.html +agent: + class: MAPPO + rollouts: 16 + learning_epochs: 8 + mini_batches: 1 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + shared_state_preprocessor: RunningStandardScaler + shared_state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cart_double_pendulum_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..7c1fd452d70 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 8 + mini_batches: 1 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cart_double_pendulum_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py new file mode 100644 index 00000000000..c4dcdfb5280 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cart_double_pendulum/cart_double_pendulum_env.py @@ -0,0 +1,231 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import torch +from collections.abc import Sequence + +from isaaclab_assets.robots.cart_double_pendulum import CART_DOUBLE_PENDULUM_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs import DirectMARLEnv, DirectMARLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils import configclass +from isaaclab.utils.math import sample_uniform + + +@configclass +class CartDoublePendulumEnvCfg(DirectMARLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + possible_agents = ["cart", "pendulum"] + action_spaces = {"cart": 1, "pendulum": 1} + observation_spaces = {"cart": 4, "pendulum": 3} + state_space = -1 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + + # robot + robot_cfg: ArticulationCfg = CART_DOUBLE_PENDULUM_CFG.replace(prim_path="/World/envs/env_.*/Robot") + cart_dof_name = "slider_to_cart" + pole_dof_name = "cart_to_pole" + pendulum_dof_name = "pole_to_pendulum" + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # reset + max_cart_pos = 3.0 # the cart is reset if it exceeds that position [m] + initial_pole_angle_range = [-0.25, 0.25] # the range in which the pole angle is sampled from on reset [rad] + initial_pendulum_angle_range = [-0.25, 0.25] # the range in which the pendulum angle is sampled from on reset [rad] + + # action scales + cart_action_scale = 100.0 # [N] + pendulum_action_scale = 50.0 # [Nm] + + # reward scales + rew_scale_alive = 1.0 + rew_scale_terminated = -2.0 + rew_scale_cart_pos = 0 + rew_scale_cart_vel = -0.01 + rew_scale_pole_pos = -1.0 + rew_scale_pole_vel = -0.01 + rew_scale_pendulum_pos = -1.0 + rew_scale_pendulum_vel = -0.01 + + +class CartDoublePendulumEnv(DirectMARLEnv): + cfg: CartDoublePendulumEnvCfg + + def __init__(self, cfg: CartDoublePendulumEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self._cart_dof_idx, _ = self.robot.find_joints(self.cfg.cart_dof_name) + self._pole_dof_idx, _ = self.robot.find_joints(self.cfg.pole_dof_name) + self._pendulum_dof_idx, _ = self.robot.find_joints(self.cfg.pendulum_dof_name) + + self.joint_pos = self.robot.data.joint_pos + self.joint_vel = self.robot.data.joint_vel + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot_cfg) + # add ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene + self.scene.articulations["robot"] = self.robot + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: dict[str, torch.Tensor]) -> None: + self.actions = actions + + def _apply_action(self) -> None: + self.robot.set_joint_effort_target( + self.actions["cart"] * self.cfg.cart_action_scale, joint_ids=self._cart_dof_idx + ) + self.robot.set_joint_effort_target( + self.actions["pendulum"] * self.cfg.pendulum_action_scale, joint_ids=self._pendulum_dof_idx + ) + + def _get_observations(self) -> dict[str, torch.Tensor]: + pole_joint_pos = normalize_angle(self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1)) + pendulum_joint_pos = normalize_angle(self.joint_pos[:, self._pendulum_dof_idx[0]].unsqueeze(dim=1)) + observations = { + "cart": torch.cat( + ( + self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + pole_joint_pos, + self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + ), + dim=-1, + ), + "pendulum": torch.cat( + ( + pole_joint_pos + pendulum_joint_pos, + pendulum_joint_pos, + self.joint_vel[:, self._pendulum_dof_idx[0]].unsqueeze(dim=1), + ), + dim=-1, + ), + } + return observations + + def _get_rewards(self) -> dict[str, torch.Tensor]: + total_reward = compute_rewards( + self.cfg.rew_scale_alive, + self.cfg.rew_scale_terminated, + self.cfg.rew_scale_cart_pos, + self.cfg.rew_scale_cart_vel, + self.cfg.rew_scale_pole_pos, + self.cfg.rew_scale_pole_vel, + self.cfg.rew_scale_pendulum_pos, + self.cfg.rew_scale_pendulum_vel, + self.joint_pos[:, self._cart_dof_idx[0]], + self.joint_vel[:, self._cart_dof_idx[0]], + normalize_angle(self.joint_pos[:, self._pole_dof_idx[0]]), + self.joint_vel[:, self._pole_dof_idx[0]], + normalize_angle(self.joint_pos[:, self._pendulum_dof_idx[0]]), + self.joint_vel[:, self._pendulum_dof_idx[0]], + math.prod(self.terminated_dict.values()), + ) + return total_reward + + def _get_dones(self) -> tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: + self.joint_pos = self.robot.data.joint_pos + self.joint_vel = self.robot.data.joint_vel + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + out_of_bounds = torch.any(torch.abs(self.joint_pos[:, self._cart_dof_idx]) > self.cfg.max_cart_pos, dim=1) + out_of_bounds = out_of_bounds | torch.any(torch.abs(self.joint_pos[:, self._pole_dof_idx]) > math.pi / 2, dim=1) + + terminated = {agent: out_of_bounds for agent in self.cfg.possible_agents} + time_outs = {agent: time_out for agent in self.cfg.possible_agents} + return terminated, time_outs + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None: + env_ids = self.robot._ALL_INDICES + super()._reset_idx(env_ids) + + joint_pos = self.robot.data.default_joint_pos[env_ids] + joint_pos[:, self._pole_dof_idx] += sample_uniform( + self.cfg.initial_pole_angle_range[0] * math.pi, + self.cfg.initial_pole_angle_range[1] * math.pi, + joint_pos[:, self._pole_dof_idx].shape, + joint_pos.device, + ) + joint_pos[:, self._pendulum_dof_idx] += sample_uniform( + self.cfg.initial_pendulum_angle_range[0] * math.pi, + self.cfg.initial_pendulum_angle_range[1] * math.pi, + joint_pos[:, self._pendulum_dof_idx].shape, + joint_pos.device, + ) + joint_vel = self.robot.data.default_joint_vel[env_ids] + + default_root_state = self.robot.data.default_root_state[env_ids] + default_root_state[:, :3] += self.scene.env_origins[env_ids] + + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + +@torch.jit.script +def normalize_angle(angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + + +@torch.jit.script +def compute_rewards( + rew_scale_alive: float, + rew_scale_terminated: float, + rew_scale_cart_pos: float, + rew_scale_cart_vel: float, + rew_scale_pole_pos: float, + rew_scale_pole_vel: float, + rew_scale_pendulum_pos: float, + rew_scale_pendulum_vel: float, + cart_pos: torch.Tensor, + cart_vel: torch.Tensor, + pole_pos: torch.Tensor, + pole_vel: torch.Tensor, + pendulum_pos: torch.Tensor, + pendulum_vel: torch.Tensor, + reset_terminated: torch.Tensor, +): + rew_alive = rew_scale_alive * (1.0 - reset_terminated.float()) + rew_termination = rew_scale_terminated * reset_terminated.float() + rew_pole_pos = rew_scale_pole_pos * torch.sum(torch.square(pole_pos).unsqueeze(dim=1), dim=-1) + rew_pendulum_pos = rew_scale_pendulum_pos * torch.sum( + torch.square(pole_pos + pendulum_pos).unsqueeze(dim=1), dim=-1 + ) + rew_cart_vel = rew_scale_cart_vel * torch.sum(torch.abs(cart_vel).unsqueeze(dim=1), dim=-1) + rew_pole_vel = rew_scale_pole_vel * torch.sum(torch.abs(pole_vel).unsqueeze(dim=1), dim=-1) + rew_pendulum_vel = rew_scale_pendulum_vel * torch.sum(torch.abs(pendulum_vel).unsqueeze(dim=1), dim=-1) + + total_reward = { + "cart": rew_alive + rew_termination + rew_pole_pos + rew_cart_vel + rew_pole_vel, + "pendulum": rew_alive + rew_termination + rew_pendulum_pos + rew_pendulum_vel, + } + return total_reward diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/__init__.py new file mode 100644 index 00000000000..65f2be06aaa --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/__init__.py @@ -0,0 +1,56 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Cartpole balancing environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Cartpole-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env:CartpoleEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CartpolePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-RGB-Camera-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env:CartpoleRGBCameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_camera_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_camera_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Depth-Camera-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env:CartpoleDepthCameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_camera_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_camera_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml new file mode 100644 index 00000000000..ee5ac79b437 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_camera_ppo_cfg.yaml @@ -0,0 +1,95 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + cnn: + type: conv2d + activation: relu + initializer: + name: default + regularizer: + name: None + convs: + - filters: 32 + kernel_size: 8 + strides: 4 + padding: 0 + - filters: 64 + kernel_size: 4 + strides: 2 + padding: 0 + - filters: 64 + kernel_size: 3 + strides: 1 + padding: 0 + + mlp: + units: [512] + activation: elu + initializer: + name: default + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole_camera_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: False + normalize_value: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 64 + minibatch_size: 2048 + mini_epochs: 4 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..e78116e70ce --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [32, 32] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.1 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 150 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 16384 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..8075eb83ba7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 150 + save_interval = 50 + experiment_name = "cartpole_direct" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[32, 32], + critic_hidden_dims=[32, 32], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml new file mode 100644 index 00000000000..5856f35f8e8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,21 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 42 + +n_timesteps: !!float 1e6 +policy: 'MlpPolicy' +n_steps: 16 +batch_size: 4096 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 20 +ent_coef: 0.01 +learning_rate: !!float 3e-4 +clip_range: !!float 0.2 +policy_kwargs: "dict( + activation_fn=nn.ELU, + net_arch=[32, 32], + squash_output=False, + )" +vf_coef: 1.0 +max_grad_norm: 1.0 +device: "cuda:0" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml new file mode 100644 index 00000000000..3e2e1958423 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml @@ -0,0 +1,96 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: features_extractor + input: permute(STATES, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(STATES, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..1efe67083a5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cartpole_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py new file mode 100644 index 00000000000..0d65c71fb23 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_camera_env.py @@ -0,0 +1,230 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import torch +from collections.abc import Sequence + +from isaaclab_assets.robots.cartpole import CARTPOLE_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg, ViewerCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import TiledCamera, TiledCameraCfg, save_images_to_file +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass +from isaaclab.utils.math import sample_uniform + + +@configclass +class CartpoleRGBCameraEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + action_scale = 100.0 # [N] + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + + # robot + robot_cfg: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="/World/envs/env_.*/Robot") + cart_dof_name = "slider_to_cart" + pole_dof_name = "cart_to_pole" + + # camera + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="/World/envs/env_.*/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-5.0, 0.0, 2.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=100, + height=100, + ) + write_image_to_file = False + + # spaces + action_space = 1 + state_space = 0 + observation_space = [tiled_camera.height, tiled_camera.width, 3] + + # change viewer settings + viewer = ViewerCfg(eye=(20.0, 20.0, 20.0)) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=512, env_spacing=20.0, replicate_physics=True) + + # reset + max_cart_pos = 3.0 # the cart is reset if it exceeds that position [m] + initial_pole_angle_range = [-0.125, 0.125] # the range in which the pole angle is sampled from on reset [rad] + + # reward scales + rew_scale_alive = 1.0 + rew_scale_terminated = -2.0 + rew_scale_pole_pos = -1.0 + rew_scale_cart_vel = -0.01 + rew_scale_pole_vel = -0.005 + + +@configclass +class CartpoleDepthCameraEnvCfg(CartpoleRGBCameraEnvCfg): + # camera + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="/World/envs/env_.*/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-5.0, 0.0, 2.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + data_types=["depth"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=100, + height=100, + ) + + # spaces + observation_space = [tiled_camera.height, tiled_camera.width, 1] + + +class CartpoleCameraEnv(DirectRLEnv): + + cfg: CartpoleRGBCameraEnvCfg | CartpoleDepthCameraEnvCfg + + def __init__( + self, cfg: CartpoleRGBCameraEnvCfg | CartpoleDepthCameraEnvCfg, render_mode: str | None = None, **kwargs + ): + super().__init__(cfg, render_mode, **kwargs) + + self._cart_dof_idx, _ = self._cartpole.find_joints(self.cfg.cart_dof_name) + self._pole_dof_idx, _ = self._cartpole.find_joints(self.cfg.pole_dof_name) + self.action_scale = self.cfg.action_scale + + self.joint_pos = self._cartpole.data.joint_pos + self.joint_vel = self._cartpole.data.joint_vel + + if len(self.cfg.tiled_camera.data_types) != 1: + raise ValueError( + "The Cartpole camera environment only supports one image type at a time but the following were" + f" provided: {self.cfg.tiled_camera.data_types}" + ) + + def close(self): + """Cleanup for the environment.""" + super().close() + + def _setup_scene(self): + """Setup the scene with the cartpole and camera.""" + self._cartpole = Articulation(self.cfg.robot_cfg) + self._tiled_camera = TiledCamera(self.cfg.tiled_camera) + + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + + # add articulation and sensors to scene + self.scene.articulations["cartpole"] = self._cartpole + self.scene.sensors["tiled_camera"] = self._tiled_camera + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = self.action_scale * actions.clone() + + def _apply_action(self) -> None: + self._cartpole.set_joint_effort_target(self.actions, joint_ids=self._cart_dof_idx) + + def _get_observations(self) -> dict: + data_type = "rgb" if "rgb" in self.cfg.tiled_camera.data_types else "depth" + if "rgb" in self.cfg.tiled_camera.data_types: + camera_data = self._tiled_camera.data.output[data_type] / 255.0 + # normalize the camera data for better training results + mean_tensor = torch.mean(camera_data, dim=(1, 2), keepdim=True) + camera_data -= mean_tensor + elif "depth" in self.cfg.tiled_camera.data_types: + camera_data = self._tiled_camera.data.output[data_type] + camera_data[camera_data == float("inf")] = 0 + observations = {"policy": camera_data.clone()} + + if self.cfg.write_image_to_file: + save_images_to_file(observations["policy"], f"cartpole_{data_type}.png") + + return observations + + def _get_rewards(self) -> torch.Tensor: + total_reward = compute_rewards( + self.cfg.rew_scale_alive, + self.cfg.rew_scale_terminated, + self.cfg.rew_scale_pole_pos, + self.cfg.rew_scale_cart_vel, + self.cfg.rew_scale_pole_vel, + self.joint_pos[:, self._pole_dof_idx[0]], + self.joint_vel[:, self._pole_dof_idx[0]], + self.joint_pos[:, self._cart_dof_idx[0]], + self.joint_vel[:, self._cart_dof_idx[0]], + self.reset_terminated, + ) + return total_reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + self.joint_pos = self._cartpole.data.joint_pos + self.joint_vel = self._cartpole.data.joint_vel + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + out_of_bounds = torch.any(torch.abs(self.joint_pos[:, self._cart_dof_idx]) > self.cfg.max_cart_pos, dim=1) + out_of_bounds = out_of_bounds | torch.any(torch.abs(self.joint_pos[:, self._pole_dof_idx]) > math.pi / 2, dim=1) + return out_of_bounds, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None: + env_ids = self._cartpole._ALL_INDICES + super()._reset_idx(env_ids) + + joint_pos = self._cartpole.data.default_joint_pos[env_ids] + joint_pos[:, self._pole_dof_idx] += sample_uniform( + self.cfg.initial_pole_angle_range[0] * math.pi, + self.cfg.initial_pole_angle_range[1] * math.pi, + joint_pos[:, self._pole_dof_idx].shape, + joint_pos.device, + ) + joint_vel = self._cartpole.data.default_joint_vel[env_ids] + + default_root_state = self._cartpole.data.default_root_state[env_ids] + default_root_state[:, :3] += self.scene.env_origins[env_ids] + + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + self._cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._cartpole.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + +@torch.jit.script +def compute_rewards( + rew_scale_alive: float, + rew_scale_terminated: float, + rew_scale_pole_pos: float, + rew_scale_cart_vel: float, + rew_scale_pole_vel: float, + pole_pos: torch.Tensor, + pole_vel: torch.Tensor, + cart_pos: torch.Tensor, + cart_vel: torch.Tensor, + reset_terminated: torch.Tensor, +): + rew_alive = rew_scale_alive * (1.0 - reset_terminated.float()) + rew_termination = rew_scale_terminated * reset_terminated.float() + rew_pole_pos = rew_scale_pole_pos * torch.sum(torch.square(pole_pos).unsqueeze(dim=1), dim=-1) + rew_cart_vel = rew_scale_cart_vel * torch.sum(torch.abs(cart_vel).unsqueeze(dim=1), dim=-1) + rew_pole_vel = rew_scale_pole_vel * torch.sum(torch.abs(pole_vel).unsqueeze(dim=1), dim=-1) + total_reward = rew_alive + rew_termination + rew_pole_pos + rew_cart_vel + rew_pole_vel + return total_reward diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_env.py new file mode 100644 index 00000000000..10a2fde99b2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole/cartpole_env.py @@ -0,0 +1,174 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import torch +from collections.abc import Sequence + +from isaaclab_assets.robots.cartpole import CARTPOLE_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils import configclass +from isaaclab.utils.math import sample_uniform + + +@configclass +class CartpoleEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + action_scale = 100.0 # [N] + action_space = 1 + observation_space = 4 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + + # robot + robot_cfg: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="/World/envs/env_.*/Robot") + cart_dof_name = "slider_to_cart" + pole_dof_name = "cart_to_pole" + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # reset + max_cart_pos = 3.0 # the cart is reset if it exceeds that position [m] + initial_pole_angle_range = [-0.25, 0.25] # the range in which the pole angle is sampled from on reset [rad] + + # reward scales + rew_scale_alive = 1.0 + rew_scale_terminated = -2.0 + rew_scale_pole_pos = -1.0 + rew_scale_cart_vel = -0.01 + rew_scale_pole_vel = -0.005 + + +class CartpoleEnv(DirectRLEnv): + cfg: CartpoleEnvCfg + + def __init__(self, cfg: CartpoleEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self._cart_dof_idx, _ = self.cartpole.find_joints(self.cfg.cart_dof_name) + self._pole_dof_idx, _ = self.cartpole.find_joints(self.cfg.pole_dof_name) + self.action_scale = self.cfg.action_scale + + self.joint_pos = self.cartpole.data.joint_pos + self.joint_vel = self.cartpole.data.joint_vel + + def _setup_scene(self): + self.cartpole = Articulation(self.cfg.robot_cfg) + # add ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene + self.scene.articulations["cartpole"] = self.cartpole + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = self.action_scale * actions.clone() + + def _apply_action(self) -> None: + self.cartpole.set_joint_effort_target(self.actions, joint_ids=self._cart_dof_idx) + + def _get_observations(self) -> dict: + obs = torch.cat( + ( + self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + ), + dim=-1, + ) + observations = {"policy": obs} + return observations + + def _get_rewards(self) -> torch.Tensor: + total_reward = compute_rewards( + self.cfg.rew_scale_alive, + self.cfg.rew_scale_terminated, + self.cfg.rew_scale_pole_pos, + self.cfg.rew_scale_cart_vel, + self.cfg.rew_scale_pole_vel, + self.joint_pos[:, self._pole_dof_idx[0]], + self.joint_vel[:, self._pole_dof_idx[0]], + self.joint_pos[:, self._cart_dof_idx[0]], + self.joint_vel[:, self._cart_dof_idx[0]], + self.reset_terminated, + ) + return total_reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + self.joint_pos = self.cartpole.data.joint_pos + self.joint_vel = self.cartpole.data.joint_vel + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + out_of_bounds = torch.any(torch.abs(self.joint_pos[:, self._cart_dof_idx]) > self.cfg.max_cart_pos, dim=1) + out_of_bounds = out_of_bounds | torch.any(torch.abs(self.joint_pos[:, self._pole_dof_idx]) > math.pi / 2, dim=1) + return out_of_bounds, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None: + env_ids = self.cartpole._ALL_INDICES + super()._reset_idx(env_ids) + + joint_pos = self.cartpole.data.default_joint_pos[env_ids] + joint_pos[:, self._pole_dof_idx] += sample_uniform( + self.cfg.initial_pole_angle_range[0] * math.pi, + self.cfg.initial_pole_angle_range[1] * math.pi, + joint_pos[:, self._pole_dof_idx].shape, + joint_pos.device, + ) + joint_vel = self.cartpole.data.default_joint_vel[env_ids] + + default_root_state = self.cartpole.data.default_root_state[env_ids] + default_root_state[:, :3] += self.scene.env_origins[env_ids] + + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + self.cartpole.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.cartpole.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.cartpole.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + +@torch.jit.script +def compute_rewards( + rew_scale_alive: float, + rew_scale_terminated: float, + rew_scale_pole_pos: float, + rew_scale_cart_vel: float, + rew_scale_pole_vel: float, + pole_pos: torch.Tensor, + pole_vel: torch.Tensor, + cart_pos: torch.Tensor, + cart_vel: torch.Tensor, + reset_terminated: torch.Tensor, +): + rew_alive = rew_scale_alive * (1.0 - reset_terminated.float()) + rew_termination = rew_scale_terminated * reset_terminated.float() + rew_pole_pos = rew_scale_pole_pos * torch.sum(torch.square(pole_pos).unsqueeze(dim=1), dim=-1) + rew_cart_vel = rew_scale_cart_vel * torch.sum(torch.abs(cart_vel).unsqueeze(dim=1), dim=-1) + rew_pole_vel = rew_scale_pole_vel * torch.sum(torch.abs(pole_vel).unsqueeze(dim=1), dim=-1) + total_reward = rew_alive + rew_termination + rew_pole_pos + rew_cart_vel + rew_pole_vel + return total_reward diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/__init__.py new file mode 100644 index 00000000000..377856f678e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Cartpole environment showcase for the supported Gymnasium spaces.""" + +from .cartpole import * # noqa +from .cartpole_camera import * # noqa diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py new file mode 100644 index 00000000000..770f5a5b164 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/__init__.py @@ -0,0 +1,191 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Cartpole balancing environment. +""" + +import gymnasium as gym + +from . import agents + +########################### +# Register Gym environments +########################### + +### +# Observation space as Box +### + +gym.register( + id="Isaac-Cartpole-Showcase-Box-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:BoxBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Box-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:BoxDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Box-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:BoxMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as Discrete +### + +gym.register( + id="Isaac-Cartpole-Showcase-Discrete-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DiscreteBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_discrete_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Discrete-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DiscreteDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_discrete_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Discrete-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DiscreteMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_discrete_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as MultiDiscrete +### + +gym.register( + id="Isaac-Cartpole-Showcase-MultiDiscrete-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:MultiDiscreteBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_multidiscrete_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-MultiDiscrete-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:MultiDiscreteDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_multidiscrete_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-MultiDiscrete-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:MultiDiscreteMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_multidiscrete_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as Dict +### + +gym.register( + id="Isaac-Cartpole-Showcase-Dict-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DictBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Dict-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DictDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Dict-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:DictMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as Tuple +### + +gym.register( + id="Isaac-Cartpole-Showcase-Tuple-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:TupleBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Tuple-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:TupleDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Showcase-Tuple-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_env:CartpoleShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:TupleMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_multidiscrete_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml new file mode 100644 index 00000000000..a616b88054b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_box_ppo_cfg.yaml @@ -0,0 +1,101 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_box_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..26dbdc6190c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_discrete_ppo_cfg.yaml @@ -0,0 +1,97 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_box_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..5f2dc924f3f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_box_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,97 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: OBSERVATIONS + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_box_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml new file mode 100644 index 00000000000..af34a744264 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_box_ppo_cfg.yaml @@ -0,0 +1,124 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["joint-positions"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(+)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_dict_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..c3cab386c86 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_discrete_ppo_cfg.yaml @@ -0,0 +1,120 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["joint-positions"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(+)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_dict_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..63d0f930569 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_dict_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,120 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["joint-positions"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(+)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS["joint-positions"] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS["joint-velocities"] + layers: [16, 16] + activations: elu + - name: net + input: net_pos + net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_dict_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml new file mode 100644 index 00000000000..d8baac3ec09 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_box_ppo_cfg.yaml @@ -0,0 +1,103 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_discrete_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..1994df77c59 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_discrete_ppo_cfg.yaml @@ -0,0 +1,99 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_discrete_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..12613e0fc6d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_discrete_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,99 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_discrete_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml new file mode 100644 index 00000000000..c02301ac341 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_box_ppo_cfg.yaml @@ -0,0 +1,103 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_multidiscrete_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..c3b22efe1c5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_discrete_ppo_cfg.yaml @@ -0,0 +1,99 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_multidiscrete_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..552f58bda26 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_multidiscrete_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,99 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# one_hot(obs) +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: one_hot_encoding(OBSERVATION_SPACE, OBSERVATIONS) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null # pre-processor should not be used with Discrete/MultiDiscrete observations + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_multidiscrete_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml new file mode 100644 index 00000000000..1213276ed4e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_box_ppo_cfg.yaml @@ -0,0 +1,124 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(*)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_tuple_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..90faeda183f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_discrete_ppo_cfg.yaml @@ -0,0 +1,120 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(*)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_tuple_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..03ef59f96ca --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,120 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━▼━━━━━┓ ┏━━━━━━▼━━━━━┓ +# ┃ net_pos ┃ ┃ net_vel ┃ +# ┡━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━┩ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# │ linear(16) │ │ linear(16) │ +# │ elu │ │ elu │ +# └──────┬─────┘ └─────┬──────┘ +# │ │ +# └─────────▶(*)◀─────────┘ +# │ +# ┏━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━┩ +# │ identity │ +# shared └─────┬─────┘ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net_pos + input: OBSERVATIONS[0] + layers: [16, 16] + activations: elu + - name: net_vel + input: OBSERVATIONS[1] + layers: [16, 16] + activations: elu + - name: net + input: net_pos * net_vel + layers: [] + activations: [] + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_direct_tuple_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py new file mode 100644 index 00000000000..7674e1accb6 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env.py @@ -0,0 +1,139 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import torch + +from isaaclab_tasks.direct.cartpole.cartpole_env import CartpoleEnv, CartpoleEnvCfg + + +class CartpoleShowcaseEnv(CartpoleEnv): + cfg: CartpoleEnvCfg + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + # fundamental spaces + # - Box + if isinstance(self.single_action_space, gym.spaces.Box): + target = self.cfg.action_scale * self.actions + # - Discrete + elif isinstance(self.single_action_space, gym.spaces.Discrete): + target = torch.zeros((self.num_envs, 1), dtype=torch.float32, device=self.device) + target = torch.where(self.actions == 1, -self.cfg.action_scale, target) + target = torch.where(self.actions == 2, self.cfg.action_scale, target) + # - MultiDiscrete + elif isinstance(self.single_action_space, gym.spaces.MultiDiscrete): + # value + target = torch.zeros((self.num_envs, 1), dtype=torch.float32, device=self.device) + target = torch.where(self.actions[:, [0]] == 1, self.cfg.action_scale / 2.0, target) + target = torch.where(self.actions[:, [0]] == 2, self.cfg.action_scale, target) + # direction + target = torch.where(self.actions[:, [1]] == 0, -target, target) + else: + raise NotImplementedError(f"Action space {type(self.single_action_space)} not implemented") + + # set target + self.cartpole.set_joint_effort_target(target, joint_ids=self._cart_dof_idx) + + def _get_observations(self) -> dict: + + # fundamental spaces + # - Box + if isinstance(self.single_observation_space["policy"], gym.spaces.Box): + obs = torch.cat( + ( + self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + ), + dim=-1, + ) + # - Discrete + elif isinstance(self.single_observation_space["policy"], gym.spaces.Discrete): + data = ( + torch.cat( + ( + self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1), + self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1), + ), + dim=-1, + ) + >= 0 + ) + obs = torch.zeros((self.num_envs,), dtype=torch.int32, device=self.device) + obs = torch.where(discretization_indices(data, [False, False, False, True]), 1, obs) + obs = torch.where(discretization_indices(data, [False, False, True, False]), 2, obs) + obs = torch.where(discretization_indices(data, [False, False, True, True]), 3, obs) + obs = torch.where(discretization_indices(data, [False, True, False, False]), 4, obs) + obs = torch.where(discretization_indices(data, [False, True, False, True]), 5, obs) + obs = torch.where(discretization_indices(data, [False, True, True, False]), 6, obs) + obs = torch.where(discretization_indices(data, [False, True, True, True]), 7, obs) + obs = torch.where(discretization_indices(data, [True, False, False, False]), 8, obs) + obs = torch.where(discretization_indices(data, [True, False, False, True]), 9, obs) + obs = torch.where(discretization_indices(data, [True, False, True, False]), 10, obs) + obs = torch.where(discretization_indices(data, [True, False, True, True]), 11, obs) + obs = torch.where(discretization_indices(data, [True, True, False, False]), 12, obs) + obs = torch.where(discretization_indices(data, [True, True, False, True]), 13, obs) + obs = torch.where(discretization_indices(data, [True, True, True, False]), 14, obs) + obs = torch.where(discretization_indices(data, [True, True, True, True]), 15, obs) + # - MultiDiscrete + elif isinstance(self.single_observation_space["policy"], gym.spaces.MultiDiscrete): + zeros = torch.zeros((self.num_envs,), dtype=torch.int32, device=self.device) + ones = torch.ones_like(zeros) + obs = torch.cat( + ( + torch.where( + discretization_indices(self.joint_pos[:, self._pole_dof_idx[0]].unsqueeze(dim=1) >= 0, [True]), + ones, + zeros, + ).unsqueeze(dim=1), + torch.where( + discretization_indices(self.joint_pos[:, self._cart_dof_idx[0]].unsqueeze(dim=1) >= 0, [True]), + ones, + zeros, + ).unsqueeze(dim=1), + torch.where( + discretization_indices(self.joint_vel[:, self._pole_dof_idx[0]].unsqueeze(dim=1) >= 0, [True]), + ones, + zeros, + ).unsqueeze(dim=1), + torch.where( + discretization_indices(self.joint_vel[:, self._cart_dof_idx[0]].unsqueeze(dim=1) >= 0, [True]), + ones, + zeros, + ).unsqueeze(dim=1), + ), + dim=-1, + ) + # composite spaces + # - Tuple + elif isinstance(self.single_observation_space["policy"], gym.spaces.Tuple): + obs = (self.joint_pos, self.joint_vel) + # - Dict + elif isinstance(self.single_observation_space["policy"], gym.spaces.Dict): + obs = {"joint-positions": self.joint_pos, "joint-velocities": self.joint_vel} + else: + raise NotImplementedError( + f"Observation space {type(self.single_observation_space['policy'])} not implemented" + ) + + observations = {"policy": obs} + return observations + + +def discretization_indices(x: torch.Tensor, condition: list[bool]) -> torch.Tensor: + return torch.prod(x == torch.tensor(condition, device=x.device), axis=-1).to(torch.bool) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py new file mode 100644 index 00000000000..544669f1cc2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole/cartpole_env_cfg.py @@ -0,0 +1,600 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from gymnasium import spaces + +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.cartpole.cartpole_env import CartpoleEnvCfg + +### +# Observation space as Box +### + + +@configclass +class BoxBoxEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (4,)) + + === === + Idx Observation + === === + 0 Pole DOF position + 1 Pole DOF velocity + 2 Cart DOF position + 3 Cart DOF velocity + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + observation_space = spaces.Box(low=float("-inf"), high=float("inf"), shape=(4,)) # or for simplicity: 4 or [4] + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class BoxDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (4,)) + + === === + Idx Observation + === === + 0 Pole DOF position + 1 Pole DOF velocity + 2 Cart DOF position + 3 Cart DOF velocity + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + observation_space = spaces.Box(low=float("-inf"), high=float("inf"), shape=(4,)) # or for simplicity: 4 or [4] + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class BoxMultiDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (4,)) + + === === + Idx Observation + === === + 0 Pole DOF position + 1 Pole DOF velocity + 2 Cart DOF position + 3 Cart DOF velocity + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + observation_space = spaces.Box(low=float("-inf"), high=float("inf"), shape=(4,)) # or for simplicity: 4 or [4] + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as Discrete +### + + +@configclass +class DiscreteBoxEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Discrete`` with 16 elements) + + === === + N Observation (Value signs: pole position, cart position, pole velocity, cart velocity) + === === + 0 - - - - + 1 - - - + + 2 - - + - + 3 - - + + + 4 - + - - + 5 - + - + + 6 - + + - + 7 - + + + + 8 + - - - + 9 + - - + + 10 + - + - + 11 + - + + + 12 + + - - + 13 + + - + + 14 + + + - + 15 + + + + + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + observation_space = spaces.Discrete(16) # or for simplicity: {16} + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class DiscreteDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Discrete`` with 16 elements) + + === === + N Observation (Value signs: pole position, cart position, pole velocity, cart velocity) + === === + 0 - - - - + 1 - - - + + 2 - - + - + 3 - - + + + 4 - + - - + 5 - + - + + 6 - + + - + 7 - + + + + 8 + - - - + 9 + - - + + 10 + - + - + 11 + - + + + 12 + + - - + 13 + + - + + 14 + + + - + 15 + + + + + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + observation_space = spaces.Discrete(16) # or for simplicity: {16} + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class DiscreteMultiDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Discrete`` with 16 elements) + + === === + N Observation (Value signs: pole position, cart position, pole velocity, cart velocity) + === === + 0 - - - - + 1 - - - + + 2 - - + - + 3 - - + + + 4 - + - - + 5 - + - + + 6 - + + - + 7 - + + + + 8 + - - - + 9 + - - + + 10 + - + - + 11 + - + + + 12 + + - - + 13 + + - + + 14 + + + - + 15 + + + + + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + observation_space = spaces.Discrete(16) # or for simplicity: {16} + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as MultiDiscrete +### + + +@configclass +class MultiDiscreteBoxEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.MultiDiscrete`` with 4 discrete spaces) + + === === + N Observation (Discrete 0) + === === + 0 Negative pole position (-) + 1 Zero or positive pole position (+) + === === + + === === + N Observation (Discrete 1) + === === + 0 Negative cart position (-) + 1 Zero or positive cart position (+) + === === + + === === + N Observation (Discrete 2) + === === + 0 Negative pole velocity (-) + 1 Zero or positive pole velocity (+) + === === + + === === + N Observation (Discrete 3) + === === + 0 Negative cart velocity (-) + 1 Zero or positive cart velocity (+) + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + observation_space = spaces.MultiDiscrete([2, 2, 2, 2]) # or for simplicity: [{2}, {2}, {2}, {2}] + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class MultiDiscreteDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.MultiDiscrete`` with 4 discrete spaces) + + === === + N Observation (Discrete 0) + === === + 0 Negative pole position (-) + 1 Zero or positive pole position (+) + === === + + === === + N Observation (Discrete 1) + === === + 0 Negative cart position (-) + 1 Zero or positive cart position (+) + === === + + === === + N Observation (Discrete 2) + === === + 0 Negative pole velocity (-) + 1 Zero or positive pole velocity (+) + === === + + === === + N Observation (Discrete 3) + === === + 0 Negative cart velocity (-) + 1 Zero or positive cart velocity (+) + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + observation_space = spaces.MultiDiscrete([2, 2, 2, 2]) # or for simplicity: [{2}, {2}, {2}, {2}] + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class MultiDiscreteMultiDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.MultiDiscrete`` with 4 discrete spaces) + + === === + N Observation (Discrete 0) + === === + 0 Negative pole position (-) + 1 Zero or positive pole position (+) + === === + + === === + N Observation (Discrete 1) + === === + 0 Negative cart position (-) + 1 Zero or positive cart position (+) + === === + + === === + N Observation (Discrete 2) + === === + 0 Negative pole velocity (-) + 1 Zero or positive pole velocity (+) + === === + + === === + N Observation (Discrete 3) + === === + 0 Negative cart velocity (-) + 1 Zero or positive cart velocity (+) + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + observation_space = spaces.MultiDiscrete([2, 2, 2, 2]) # or for simplicity: [{2}, {2}, {2}, {2}] + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as Dict +### + + +@configclass +class DictBoxEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-positions DOF positions + joint-velocities DOF velocities + ================ === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + observation_space = spaces.Dict({ + "joint-positions": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + }) # or for simplicity: {"joint-positions": 2, "joint-velocities": 2} + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class DictDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-positions DOF positions + joint-velocities DOF velocities + ================ === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + observation_space = spaces.Dict({ + "joint-positions": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + }) # or for simplicity: {"joint-positions": 2, "joint-velocities": 2} + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class DictMultiDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-positions DOF positions + joint-velocities DOF velocities + ================ === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + observation_space = spaces.Dict({ + "joint-positions": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + }) # or for simplicity: {"joint-positions": 2, "joint-velocities": 2} + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as Tuple +### + + +@configclass +class TupleBoxEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 DOF positions + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: (2, 2) + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class TupleDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 DOF positions + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: (2, 2) + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class TupleMultiDiscreteEnvCfg(CartpoleEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 DOF positions + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: (2, 2) + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py new file mode 100644 index 00000000000..994c04ca6b0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/__init__.py @@ -0,0 +1,123 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Cartpole balancing environment with camera. +""" + +import gymnasium as gym + +from . import agents + +########################### +# Register Gym environments +########################### + +### +# Observation space as Box +### + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Box-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:BoxBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Box-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:BoxDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Box-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:BoxMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_box_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as Dict +### + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Dict-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:DictBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Dict-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:DictDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Dict-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:DictMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_dict_multidiscrete_ppo_cfg.yaml", + }, +) + +### +# Observation space as Tuple +### + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Tuple-Box-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:TupleBoxEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_box_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Tuple-Discrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:TupleDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_discrete_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Camera-Showcase-Tuple-MultiDiscrete-Direct-v0", + entry_point=f"{__name__}.cartpole_camera_env:CartpoleCameraShowcaseEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:TupleMultiDiscreteEnvCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_tuple_multidiscrete_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml new file mode 100644 index 00000000000..082c149b4cf --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_box_ppo_cfg.yaml @@ -0,0 +1,127 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ +# ┃ features_extractor ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ +# │ relu │ +# │ conv2d(64, 4, 2) │ +# │ relu │ +# │ conv2d(64, 3, 1) │ +# │ relu │ +# │ flatten │ +# └──────────┬──────────┘ +# │ +# ┏━━━━━━▼━━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━━┩ +# │ linear(512) │ +# │ elu │ +# └──────┬──────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_box_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..4aec971cc56 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_discrete_ppo_cfg.yaml @@ -0,0 +1,123 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ +# ┃ features_extractor ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ +# │ relu │ +# │ conv2d(64, 4, 2) │ +# │ relu │ +# │ conv2d(64, 3, 1) │ +# │ relu │ +# │ flatten │ +# └──────────┬──────────┘ +# │ +# ┏━━━━━━▼━━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━━┩ +# │ linear(512) │ +# │ elu │ +# └──────┬──────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_box_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..d633ea14011 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_box_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,123 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs +# │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ +# ┃ features_extractor ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ +# │ relu │ +# │ conv2d(64, 4, 2) │ +# │ relu │ +# │ conv2d(64, 3, 1) │ +# │ relu │ +# │ flatten │ +# └──────────┬──────────┘ +# │ +# ┏━━━━━━▼━━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━━┩ +# │ linear(512) │ +# │ elu │ +# └──────┬──────┘ +# shared │ +# ......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + activations: relu + - name: net + input: features_extractor + layers: [512] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_box_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml new file mode 100644 index 00000000000..89b88c175d2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_box_ppo_cfg.yaml @@ -0,0 +1,139 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["camera"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ │ +# ┃ features_extractor ┃ │ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ │ +# │ conv2d(32, 8, 4) │ │ +# │ relu │ │ +# │ conv2d(64, 4, 2) │ │ +# │ relu │ │ +# │ conv2d(64, 3, 1) │ │ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀────────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: features_extractor + input: permute(OBSERVATIONS["camera"], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_dict_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..68be1bf352b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_discrete_ppo_cfg.yaml @@ -0,0 +1,135 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["camera"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ │ +# ┃ features_extractor ┃ │ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ │ +# │ conv2d(32, 8, 4) │ │ +# │ relu │ │ +# │ conv2d(64, 4, 2) │ │ +# │ relu │ │ +# │ conv2d(64, 3, 1) │ │ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀────────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS["camera"], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_dict_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..77aa2cb78d3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_dict_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,135 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs["camera"] obs["joint-velocities"] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ │ +# ┃ features_extractor ┃ │ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ │ +# │ conv2d(32, 8, 4) │ │ +# │ relu │ │ +# │ conv2d(64, 4, 2) │ │ +# │ relu │ │ +# │ conv2d(64, 3, 1) │ │ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀────────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS["camera"], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: net + input: concatenate([features_extractor, OBSERVATIONS["joint-velocities"]]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_dict_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml new file mode 100644 index 00000000000..d1d5109c977 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_box_ppo_cfg.yaml @@ -0,0 +1,147 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ ┏━━━━━━━▼━━━━━━━━┓ +# ┃ features_extractor ┃ ┃ proprioception ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ │ linear(16) │ +# │ relu │ │ elu │ +# │ conv2d(64, 4, 2) │ │ linear(8) │ +# │ relu │ │ elu │ +# │ conv2d(64, 3, 1) │ └───────┬────────┘ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀───────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_tuple_box" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml new file mode 100644 index 00000000000..eb46117a0a1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_discrete_ppo_cfg.yaml @@ -0,0 +1,143 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ ┏━━━━━━━▼━━━━━━━━┓ +# ┃ features_extractor ┃ ┃ proprioception ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ │ linear(16) │ +# │ relu │ │ elu │ +# │ conv2d(64, 4, 2) │ │ linear(8) │ +# │ relu │ │ elu │ +# │ conv2d(64, 3, 1) │ └───────┬────────┘ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀───────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see categorical_model parameters + class: CategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_tuple_discrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml new file mode 100644 index 00000000000..9e1b82a08a4 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/agents/skrl_tuple_multidiscrete_ppo_cfg.yaml @@ -0,0 +1,143 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +# +# obs[0] obs[1] +# │ │ +# ┏━━━━━━━━━━▼━━━━━━━━━━┓ ┏━━━━━━━▼━━━━━━━━┓ +# ┃ features_extractor ┃ ┃ proprioception ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━┩ ┡━━━━━━━━━━━━━━━━┩ +# │ conv2d(32, 8, 4) │ │ linear(16) │ +# │ relu │ │ elu │ +# │ conv2d(64, 4, 2) │ │ linear(8) │ +# │ relu │ │ elu │ +# │ conv2d(64, 3, 1) │ └───────┬────────┘ +# │ relu │ │ +# │ flatten │ │ +# │ linear(512) │ │ +# │ tanh │ │ +# │ linear(16) │ │ +# │ tanh │ │ +# └──────────┬──────────┘ | +# │ │ +# └─▶(concatenate)◀───────┘ +# │ +# ┏━━━━━━▼━━━━━┓ +# ┃ net ┃ +# ┡━━━━━━━━━━━━┩ +# │ linear(32) │ +# │ elu │ +# │ linear(32) │ +# │ elu │ +# └──────┬─────┘ +# shared │ +# .......................│....................... +# non-shared │ +# ┏━━━━━━━━━━━▼━━━━━━━━━━━┓ +# ┃ policy|value output ┃ +# ┡━━━━━━━━━━━━━━━━━━━━━━━┩ +# │ linear(num_actions|1) │ +# └───────────┬───────────┘ +# ▼ +models: + separate: False + policy: # see multicategorical_model parameters + class: MultiCategoricalMixin + unnormalized_log_prob: True + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: features_extractor + input: permute(OBSERVATIONS[0], (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + layers: + - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} + - conv2d: {out_channels: 64, kernel_size: 3, stride: 1, padding: 0} + - flatten + - linear: 512 + - linear: 16 + activations: [relu, relu, relu, null, tanh, tanh] + - name: proprioception + input: OBSERVATIONS[1] + layers: [16, 8] + activations: elu + - name: net + input: concatenate([features_extractor, proprioception]) + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 64 + learning_epochs: 4 + mini_batches: 32 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + mixed_precision: False + # logging and checkpoint + experiment: + directory: "cartpole_camera_direct_tuple_multidiscrete" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py new file mode 100644 index 00000000000..de6ac37eaf7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import torch + +from isaaclab_tasks.direct.cartpole.cartpole_camera_env import CartpoleCameraEnv, CartpoleRGBCameraEnvCfg + + +class CartpoleCameraShowcaseEnv(CartpoleCameraEnv): + cfg: CartpoleRGBCameraEnvCfg + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + # fundamental spaces + # - Box + if isinstance(self.single_action_space, gym.spaces.Box): + target = self.cfg.action_scale * self.actions + # - Discrete + elif isinstance(self.single_action_space, gym.spaces.Discrete): + target = torch.zeros((self.num_envs, 1), dtype=torch.float32, device=self.device) + target = torch.where(self.actions == 1, -self.cfg.action_scale, target) + target = torch.where(self.actions == 2, self.cfg.action_scale, target) + # - MultiDiscrete + elif isinstance(self.single_action_space, gym.spaces.MultiDiscrete): + # value + target = torch.zeros((self.num_envs, 1), dtype=torch.float32, device=self.device) + target = torch.where(self.actions[:, [0]] == 1, self.cfg.action_scale / 2.0, target) + target = torch.where(self.actions[:, [0]] == 2, self.cfg.action_scale, target) + # direction + target = torch.where(self.actions[:, [1]] == 0, -target, target) + else: + raise NotImplementedError(f"Action space {type(self.single_action_space)} not implemented") + + # set target + self._cartpole.set_joint_effort_target(target, joint_ids=self._cart_dof_idx) + + def _get_observations(self) -> dict: + # get camera data + data_type = "rgb" if "rgb" in self.cfg.tiled_camera.data_types else "depth" + if "rgb" in self.cfg.tiled_camera.data_types: + camera_data = self._tiled_camera.data.output[data_type] / 255.0 + # normalize the camera data for better training results + mean_tensor = torch.mean(camera_data, dim=(1, 2), keepdim=True) + camera_data -= mean_tensor + elif "depth" in self.cfg.tiled_camera.data_types: + camera_data = self._tiled_camera.data.output[data_type] + camera_data[camera_data == float("inf")] = 0 + + # fundamental spaces + # - Box + if isinstance(self.single_observation_space["policy"], gym.spaces.Box): + obs = camera_data + # composite spaces + # - Tuple + elif isinstance(self.single_observation_space["policy"], gym.spaces.Tuple): + obs = (camera_data, self.joint_vel) + # - Dict + elif isinstance(self.single_observation_space["policy"], gym.spaces.Dict): + obs = {"joint-velocities": self.joint_vel, "camera": camera_data} + else: + raise NotImplementedError( + f"Observation space {type(self.single_observation_space['policy'])} not implemented" + ) + + observations = {"policy": obs} + return observations diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py new file mode 100644 index 00000000000..81fa1d59dbb --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/cartpole_showcase/cartpole_camera/cartpole_camera_env_cfg.py @@ -0,0 +1,362 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from gymnasium import spaces + +import isaaclab.sim as sim_utils +from isaaclab.sensors import TiledCameraCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.cartpole.cartpole_camera_env import CartpoleRGBCameraEnvCfg as CartpoleCameraEnvCfg + + +def get_tiled_camera_cfg(data_type: str, width: int = 100, height: int = 100) -> TiledCameraCfg: + return TiledCameraCfg( + prim_path="/World/envs/env_.*/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-5.0, 0.0, 2.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"), + data_types=[data_type], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=width, + height=height, + ) + + +### +# Observation space as Box +### + + +@configclass +class BoxBoxEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (height, width, 3)) + + === === + Idx Observation + === === + - RGB image + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Box( + low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3) + ) # or for simplicity: [height, width, 3] + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class BoxDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (height, width, 3)) + + === === + Idx Observation + === === + - RGB image + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Box( + low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3) + ) # or for simplicity: [height, width, 3] + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class BoxMultiDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Box`` with shape (height, width, 3)) + + === === + Idx Observation + === === + - RGB image + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Box( + low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3) + ) # or for simplicity: [height, width, 3] + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as Dict +### + + +@configclass +class DictBoxEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-velocities DOF velocities + camera RGB image + ================ === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Dict({ + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "camera": spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + }) # or for simplicity: {"joint-velocities": 2, "camera": [height, width, 3]} + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class DictDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-velocities DOF velocities + camera RGB image + ================ === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Dict({ + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "camera": spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + }) # or for simplicity: {"joint-velocities": 2, "camera": [height, width, 3]} + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class DictMultiDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Dict`` with 2 constituent spaces) + + ================ === + Key Observation + ================ === + joint-velocities DOF velocities + camera RGB image + ================ === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Dict({ + "joint-velocities": spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + "camera": spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + }) # or for simplicity: {"joint-velocities": 2, "camera": [height, width, 3]} + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] + + +### +# Observation space as Tuple +### + + +@configclass +class TupleBoxEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 RGB image + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.Box`` with shape (1,)) + + === === + Idx Action + === === + 0 Cart DOF effort scale: [-1, 1] + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: ([height, width, 3], 2) + action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,)) # or for simplicity: 1 or [1] + + +@configclass +class TupleDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 RGB image + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.Discrete`` with 3 elements) + + === === + N Action + === === + 0 Zero cart DOF effort + 1 Negative maximum cart DOF effort + 2 Positive maximum cart DOF effort + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: ([height, width, 3], 2) + action_space = spaces.Discrete(3) # or for simplicity: {3} + + +@configclass +class TupleMultiDiscreteEnvCfg(CartpoleCameraEnvCfg): + """ + * Observation space (``~gymnasium.spaces.Tuple`` with 2 constituent spaces) + + === === + Idx Observation + === === + 0 RGB image + 1 DOF velocities + === === + + * Action space (``~gymnasium.spaces.MultiDiscrete`` with 2 discrete spaces) + + === === + N Action (Discrete 0) + === === + 0 Zero cart DOF effort + 1 Half of maximum cart DOF effort + 2 Maximum cart DOF effort + === === + + === === + N Action (Discrete 1) + === === + 0 Negative effort (one side) + 1 Positive effort (other side) + === === + """ + + # camera + tiled_camera: TiledCameraCfg = get_tiled_camera_cfg("rgb") + + # spaces + observation_space = spaces.Tuple(( + spaces.Box(low=float("-inf"), high=float("inf"), shape=(tiled_camera.height, tiled_camera.width, 3)), + spaces.Box(low=float("-inf"), high=float("inf"), shape=(2,)), + )) # or for simplicity: ([height, width, 3], 2) + action_space = spaces.MultiDiscrete([3, 2]) # or for simplicity: [{3}, {2}] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/__init__.py new file mode 100644 index 00000000000..76e455c616c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/__init__.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents +from .factory_env import FactoryEnv +from .factory_env_cfg import FactoryTaskGearMeshCfg, FactoryTaskNutThreadCfg, FactoryTaskPegInsertCfg + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Factory-PegInsert-Direct-v0", + entry_point="isaaclab_tasks.direct.factory:FactoryEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": FactoryTaskPegInsertCfg, + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Factory-GearMesh-Direct-v0", + entry_point="isaaclab_tasks.direct.factory:FactoryEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": FactoryTaskGearMeshCfg, + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Factory-NutThread-Direct-v0", + entry_point="isaaclab_tasks.direct.factory:FactoryEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": FactoryTaskNutThreadCfg, + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..5494199846b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,118 @@ +params: + seed: 0 + algo: + name: a2c_continuous + + env: + clip_actions: 1.0 + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: False + mlp: + units: [512, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + rnn: + name: lstm + units: 1024 + layers: 2 + before_mlp: True + concat_input: True + layer_norm: True + + load_checkpoint: False + load_path: "" + + config: + name: Factory + device: cuda:0 + full_experiment_name: test + env_name: rlgpu + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: 128 + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.995 + tau: 0.95 + learning_rate: 1.0e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 200 + save_best_after: 10 + save_frequency: 100 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 # 0.0001 # 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 128 + minibatch_size: 512 # batch size = num_envs * horizon_length; minibatch_size = batch_size / num_minibatches + mini_epochs: 4 + critic_coef: 2 + clip_value: True + seq_length: 128 + bounds_loss_coef: 0.0001 + + central_value_config: + minibatch_size: 512 + mini_epochs: 4 + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + clip_value: True + normalize_input: True + truncate_grads: True + + network: + name: actor_critic + central_value: True + + mlp: + units: [512, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + rnn: + name: lstm + units: 1024 + layers: 2 + before_mlp: True + concat_input: True + layer_norm: True + + player: + deterministic: False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_control.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_control.py new file mode 100644 index 00000000000..a88f190ca00 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_control.py @@ -0,0 +1,201 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Factory: control module. + +Imported by base, environment, and task classes. Not directly executed. +""" + +import math +import torch + +import isaacsim.core.utils.torch as torch_utils + +from isaaclab.utils.math import axis_angle_from_quat + + +def compute_dof_torque( + cfg, + dof_pos, + dof_vel, + fingertip_midpoint_pos, + fingertip_midpoint_quat, + fingertip_midpoint_linvel, + fingertip_midpoint_angvel, + jacobian, + arm_mass_matrix, + ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat, + task_prop_gains, + task_deriv_gains, + device, +): + """Compute Franka DOF torque to move fingertips towards target pose.""" + # References: + # 1) https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf + # 2) Modern Robotics + + num_envs = cfg.scene.num_envs + dof_torque = torch.zeros((num_envs, dof_pos.shape[1]), device=device) + task_wrench = torch.zeros((num_envs, 6), device=device) + + pos_error, axis_angle_error = get_pose_error( + fingertip_midpoint_pos=fingertip_midpoint_pos, + fingertip_midpoint_quat=fingertip_midpoint_quat, + ctrl_target_fingertip_midpoint_pos=ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=ctrl_target_fingertip_midpoint_quat, + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + delta_fingertip_pose = torch.cat((pos_error, axis_angle_error), dim=1) + + # Set tau = k_p * task_pos_error - k_d * task_vel_error (building towards eq. 3.96-3.98) + task_wrench_motion = _apply_task_space_gains( + delta_fingertip_pose=delta_fingertip_pose, + fingertip_midpoint_linvel=fingertip_midpoint_linvel, + fingertip_midpoint_angvel=fingertip_midpoint_angvel, + task_prop_gains=task_prop_gains, + task_deriv_gains=task_deriv_gains, + ) + task_wrench += task_wrench_motion + + # Set tau = J^T * tau, i.e., map tau into joint space as desired + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + dof_torque[:, 0:7] = (jacobian_T @ task_wrench.unsqueeze(-1)).squeeze(-1) + + # adapted from https://gitlab-master.nvidia.com/carbon-gym/carbgym/-/blob/b4bbc66f4e31b1a1bee61dbaafc0766bbfbf0f58/python/examples/franka_cube_ik_osc.py#L70-78 + # roboticsproceedings.org/rss07/p31.pdf + + # useful tensors + arm_mass_matrix_inv = torch.inverse(arm_mass_matrix) + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + arm_mass_matrix_task = torch.inverse( + jacobian @ torch.inverse(arm_mass_matrix) @ jacobian_T + ) # ETH eq. 3.86; geometric Jacobian is assumed + j_eef_inv = arm_mass_matrix_task @ jacobian @ arm_mass_matrix_inv + default_dof_pos_tensor = torch.tensor(cfg.ctrl.default_dof_pos_tensor, device=device).repeat((num_envs, 1)) + # nullspace computation + distance_to_default_dof_pos = default_dof_pos_tensor - dof_pos[:, :7] + distance_to_default_dof_pos = (distance_to_default_dof_pos + math.pi) % ( + 2 * math.pi + ) - math.pi # normalize to [-pi, pi] + u_null = cfg.ctrl.kd_null * -dof_vel[:, :7] + cfg.ctrl.kp_null * distance_to_default_dof_pos + u_null = arm_mass_matrix @ u_null.unsqueeze(-1) + torque_null = (torch.eye(7, device=device).unsqueeze(0) - torch.transpose(jacobian, 1, 2) @ j_eef_inv) @ u_null + dof_torque[:, 0:7] += torque_null.squeeze(-1) + + # TODO: Verify it's okay to no longer do gripper control here. + dof_torque = torch.clamp(dof_torque, min=-100.0, max=100.0) + return dof_torque, task_wrench + + +def get_pose_error( + fingertip_midpoint_pos, + fingertip_midpoint_quat, + ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat, + jacobian_type, + rot_error_type, +): + """Compute task-space error between target Franka fingertip pose and current pose.""" + # Reference: https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf + + # Compute pos error + pos_error = ctrl_target_fingertip_midpoint_pos - fingertip_midpoint_pos + + # Compute rot error + if jacobian_type == "geometric": # See example 2.9.8; note use of J_g and transformation between rotation vectors + # Compute quat error (i.e., difference quat) + # Reference: https://personal.utdallas.edu/~sxb027100/dock/quat.html + + # Check for shortest path using quaternion dot product. + quat_dot = (ctrl_target_fingertip_midpoint_quat * fingertip_midpoint_quat).sum(dim=1, keepdim=True) + ctrl_target_fingertip_midpoint_quat = torch.where( + quat_dot.expand(-1, 4) >= 0, ctrl_target_fingertip_midpoint_quat, -ctrl_target_fingertip_midpoint_quat + ) + + fingertip_midpoint_quat_norm = torch_utils.quat_mul( + fingertip_midpoint_quat, torch_utils.quat_conjugate(fingertip_midpoint_quat) + )[ + :, 0 + ] # scalar component + fingertip_midpoint_quat_inv = torch_utils.quat_conjugate( + fingertip_midpoint_quat + ) / fingertip_midpoint_quat_norm.unsqueeze(-1) + quat_error = torch_utils.quat_mul(ctrl_target_fingertip_midpoint_quat, fingertip_midpoint_quat_inv) + + # Convert to axis-angle error + axis_angle_error = axis_angle_from_quat(quat_error) + + if rot_error_type == "quat": + return pos_error, quat_error + elif rot_error_type == "axis_angle": + return pos_error, axis_angle_error + + +def _get_delta_dof_pos(delta_pose, ik_method, jacobian, device): + """Get delta Franka DOF position from delta pose using specified IK method.""" + # References: + # 1) https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf + # 2) https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf (p. 47) + + if ik_method == "pinv": # Jacobian pseudoinverse + k_val = 1.0 + jacobian_pinv = torch.linalg.pinv(jacobian) + delta_dof_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "trans": # Jacobian transpose + k_val = 1.0 + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + delta_dof_pos = k_val * jacobian_T @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "dls": # damped least squares (Levenberg-Marquardt) + lambda_val = 0.1 # 0.1 + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=device) + delta_dof_pos = jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + elif ik_method == "svd": # adaptive SVD + k_val = 1.0 + U, S, Vh = torch.linalg.svd(jacobian) + S_inv = 1.0 / S + min_singular_value = 1.0e-5 + S_inv = torch.where(S > min_singular_value, S_inv, torch.zeros_like(S_inv)) + jacobian_pinv = ( + torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] @ torch.diag_embed(S_inv) @ torch.transpose(U, dim0=1, dim1=2) + ) + delta_dof_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_dof_pos = delta_dof_pos.squeeze(-1) + + return delta_dof_pos + + +def _apply_task_space_gains( + delta_fingertip_pose, fingertip_midpoint_linvel, fingertip_midpoint_angvel, task_prop_gains, task_deriv_gains +): + """Interpret PD gains as task-space gains. Apply to task-space error.""" + + task_wrench = torch.zeros_like(delta_fingertip_pose) + + # Apply gains to lin error components + lin_error = delta_fingertip_pose[:, 0:3] + task_wrench[:, 0:3] = task_prop_gains[:, 0:3] * lin_error + task_deriv_gains[:, 0:3] * ( + 0.0 - fingertip_midpoint_linvel + ) + + # Apply gains to rot error components + rot_error = delta_fingertip_pose[:, 3:6] + task_wrench[:, 3:6] = task_prop_gains[:, 3:6] * rot_error + task_deriv_gains[:, 3:6] * ( + 0.0 - fingertip_midpoint_angvel + ) + return task_wrench diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env.py new file mode 100644 index 00000000000..5abab283b81 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env.py @@ -0,0 +1,888 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import torch + +import carb +import isaacsim.core.utils.torch as torch_utils + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import axis_angle_from_quat + +from . import factory_control as fc +from .factory_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, FactoryEnvCfg + + +class FactoryEnv(DirectRLEnv): + cfg: FactoryEnvCfg + + def __init__(self, cfg: FactoryEnvCfg, render_mode: str | None = None, **kwargs): + # Update number of obs/states + cfg.observation_space = sum([OBS_DIM_CFG[obs] for obs in cfg.obs_order]) + cfg.state_space = sum([STATE_DIM_CFG[state] for state in cfg.state_order]) + cfg.observation_space += cfg.action_space + cfg.state_space += cfg.action_space + self.cfg_task = cfg.task + + super().__init__(cfg, render_mode, **kwargs) + + self._set_body_inertias() + self._init_tensors() + self._set_default_dynamics_parameters() + self._compute_intermediate_values(dt=self.physics_dt) + + def _set_body_inertias(self): + """Note: this is to account for the asset_options.armature parameter in IGE.""" + inertias = self._robot.root_physx_view.get_inertias() + offset = torch.zeros_like(inertias) + offset[:, :, [0, 4, 8]] += 0.01 + new_inertias = inertias + offset + self._robot.root_physx_view.set_inertias(new_inertias, torch.arange(self.num_envs)) + + def _set_default_dynamics_parameters(self): + """Set parameters defining dynamic interactions.""" + self.default_gains = torch.tensor(self.cfg.ctrl.default_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + + self.pos_threshold = torch.tensor(self.cfg.ctrl.pos_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + self.rot_threshold = torch.tensor(self.cfg.ctrl.rot_action_threshold, device=self.device).repeat( + (self.num_envs, 1) + ) + + # Set masses and frictions. + self._set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction) + self._set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction) + self._set_friction(self._robot, self.cfg_task.robot_cfg.friction) + + def _set_friction(self, asset, value): + """Update material properties for a given asset.""" + materials = asset.root_physx_view.get_material_properties() + materials[..., 0] = value # Static friction. + materials[..., 1] = value # Dynamic friction. + env_ids = torch.arange(self.scene.num_envs, device="cpu") + asset.root_physx_view.set_material_properties(materials, env_ids) + + def _init_tensors(self): + """Initialize tensors once.""" + self.identity_quat = ( + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + ) + + # Control targets. + self.ctrl_target_joint_pos = torch.zeros((self.num_envs, self._robot.num_joints), device=self.device) + self.ctrl_target_fingertip_midpoint_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.ctrl_target_fingertip_midpoint_quat = torch.zeros((self.num_envs, 4), device=self.device) + + # Fixed asset. + self.fixed_pos_action_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.fixed_pos_obs_frame = torch.zeros((self.num_envs, 3), device=self.device) + self.init_fixed_pos_obs_noise = torch.zeros((self.num_envs, 3), device=self.device) + + # Held asset + held_base_x_offset = 0.0 + if self.cfg_task.name == "peg_insert": + held_base_z_offset = 0.0 + elif self.cfg_task.name == "gear_mesh": + gear_base_offset = self._get_target_gear_base_offset() + held_base_x_offset = gear_base_offset[0] + held_base_z_offset = gear_base_offset[2] + elif self.cfg_task.name == "nut_thread": + held_base_z_offset = self.cfg_task.fixed_asset_cfg.base_height + else: + raise NotImplementedError("Task not implemented") + + self.held_base_pos_local = torch.tensor([0.0, 0.0, 0.0], device=self.device).repeat((self.num_envs, 1)) + self.held_base_pos_local[:, 0] = held_base_x_offset + self.held_base_pos_local[:, 2] = held_base_z_offset + self.held_base_quat_local = self.identity_quat.clone().detach() + + self.held_base_pos = torch.zeros_like(self.held_base_pos_local) + self.held_base_quat = self.identity_quat.clone().detach() + + # Computer body indices. + self.left_finger_body_idx = self._robot.body_names.index("panda_leftfinger") + self.right_finger_body_idx = self._robot.body_names.index("panda_rightfinger") + self.fingertip_body_idx = self._robot.body_names.index("panda_fingertip_centered") + + # Tensors for finite-differencing. + self.last_update_timestamp = 0.0 # Note: This is for finite differencing body velocities. + self.prev_fingertip_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.prev_fingertip_quat = self.identity_quat.clone() + self.prev_joint_pos = torch.zeros((self.num_envs, 7), device=self.device) + + # Keypoint tensors. + self.target_held_base_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.target_held_base_quat = self.identity_quat.clone().detach() + + offsets = self._get_keypoint_offsets(self.cfg_task.num_keypoints) + self.keypoint_offsets = offsets * self.cfg_task.keypoint_scale + self.keypoints_held = torch.zeros((self.num_envs, self.cfg_task.num_keypoints, 3), device=self.device) + self.keypoints_fixed = torch.zeros_like(self.keypoints_held, device=self.device) + + # Used to compute target poses. + self.fixed_success_pos_local = torch.zeros((self.num_envs, 3), device=self.device) + if self.cfg_task.name == "peg_insert": + self.fixed_success_pos_local[:, 2] = 0.0 + elif self.cfg_task.name == "gear_mesh": + gear_base_offset = self._get_target_gear_base_offset() + self.fixed_success_pos_local[:, 0] = gear_base_offset[0] + self.fixed_success_pos_local[:, 2] = gear_base_offset[2] + elif self.cfg_task.name == "nut_thread": + head_height = self.cfg_task.fixed_asset_cfg.base_height + shank_length = self.cfg_task.fixed_asset_cfg.height + thread_pitch = self.cfg_task.fixed_asset_cfg.thread_pitch + self.fixed_success_pos_local[:, 2] = head_height + shank_length - thread_pitch * 1.5 + else: + raise NotImplementedError("Task not implemented") + + self.ep_succeeded = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + self.ep_success_times = torch.zeros((self.num_envs,), dtype=torch.long, device=self.device) + + def _get_keypoint_offsets(self, num_keypoints): + """Get uniformly-spaced keypoints along a line of unit length, centered at 0.""" + keypoint_offsets = torch.zeros((num_keypoints, 3), device=self.device) + keypoint_offsets[:, -1] = torch.linspace(0.0, 1.0, num_keypoints, device=self.device) - 0.5 + + return keypoint_offsets + + def _setup_scene(self): + """Initialize simulation scene.""" + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg(), translation=(0.0, 0.0, -1.05)) + + # spawn a usd file of a table into the scene + cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") + cfg.func( + "/World/envs/env_.*/Table", cfg, translation=(0.55, 0.0, 0.0), orientation=(0.70711, 0.0, 0.0, 0.70711) + ) + + self._robot = Articulation(self.cfg.robot) + self._fixed_asset = Articulation(self.cfg_task.fixed_asset) + self._held_asset = Articulation(self.cfg_task.held_asset) + if self.cfg_task.name == "gear_mesh": + self._small_gear_asset = Articulation(self.cfg_task.small_gear_cfg) + self._large_gear_asset = Articulation(self.cfg_task.large_gear_cfg) + + self.scene.clone_environments(copy_from_source=False) + + self.scene.articulations["robot"] = self._robot + self.scene.articulations["fixed_asset"] = self._fixed_asset + self.scene.articulations["held_asset"] = self._held_asset + if self.cfg_task.name == "gear_mesh": + self.scene.articulations["small_gear"] = self._small_gear_asset + self.scene.articulations["large_gear"] = self._large_gear_asset + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _compute_intermediate_values(self, dt): + """Get values computed from raw tensors. This includes adding noise.""" + # TODO: A lot of these can probably only be set once? + self.fixed_pos = self._fixed_asset.data.root_pos_w - self.scene.env_origins + self.fixed_quat = self._fixed_asset.data.root_quat_w + + self.held_pos = self._held_asset.data.root_pos_w - self.scene.env_origins + self.held_quat = self._held_asset.data.root_quat_w + + self.fingertip_midpoint_pos = self._robot.data.body_pos_w[:, self.fingertip_body_idx] - self.scene.env_origins + self.fingertip_midpoint_quat = self._robot.data.body_quat_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w[:, self.fingertip_body_idx] + self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w[:, self.fingertip_body_idx] + + jacobians = self._robot.root_physx_view.get_jacobians() + + self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] + self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] + self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 + self.arm_mass_matrix = self._robot.root_physx_view.get_generalized_mass_matrices()[:, 0:7, 0:7] + self.joint_pos = self._robot.data.joint_pos.clone() + self.joint_vel = self._robot.data.joint_vel.clone() + + # Finite-differencing results in more reliable velocity estimates. + self.ee_linvel_fd = (self.fingertip_midpoint_pos - self.prev_fingertip_pos) / dt + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + + # Add state differences if velocity isn't being added. + rot_diff_quat = torch_utils.quat_mul( + self.fingertip_midpoint_quat, torch_utils.quat_conjugate(self.prev_fingertip_quat) + ) + rot_diff_quat *= torch.sign(rot_diff_quat[:, 0]).unsqueeze(-1) + rot_diff_aa = axis_angle_from_quat(rot_diff_quat) + self.ee_angvel_fd = rot_diff_aa / dt + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + joint_diff = self.joint_pos[:, 0:7] - self.prev_joint_pos + self.joint_vel_fd = joint_diff / dt + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + + # Keypoint tensors. + self.held_base_quat[:], self.held_base_pos[:] = torch_utils.tf_combine( + self.held_quat, self.held_pos, self.held_base_quat_local, self.held_base_pos_local + ) + self.target_held_base_quat[:], self.target_held_base_pos[:] = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, self.fixed_success_pos_local + ) + + # Compute pos of keypoints on held asset, and fixed asset in world frame + for idx, keypoint_offset in enumerate(self.keypoint_offsets): + self.keypoints_held[:, idx] = torch_utils.tf_combine( + self.held_base_quat, self.held_base_pos, self.identity_quat, keypoint_offset.repeat(self.num_envs, 1) + )[1] + self.keypoints_fixed[:, idx] = torch_utils.tf_combine( + self.target_held_base_quat, + self.target_held_base_pos, + self.identity_quat, + keypoint_offset.repeat(self.num_envs, 1), + )[1] + + self.keypoint_dist = torch.norm(self.keypoints_held - self.keypoints_fixed, p=2, dim=-1).mean(-1) + self.last_update_timestamp = self._robot._data._sim_timestamp + + def _get_observations(self): + """Get actor/critic inputs using asymmetric critic.""" + noisy_fixed_pos = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + + prev_actions = self.actions.clone() + + obs_dict = { + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_pos_rel_fixed": self.fingertip_midpoint_pos - noisy_fixed_pos, + "fingertip_quat": self.fingertip_midpoint_quat, + "ee_linvel": self.ee_linvel_fd, + "ee_angvel": self.ee_angvel_fd, + "prev_actions": prev_actions, + } + + state_dict = { + "fingertip_pos": self.fingertip_midpoint_pos, + "fingertip_pos_rel_fixed": self.fingertip_midpoint_pos - self.fixed_pos_obs_frame, + "fingertip_quat": self.fingertip_midpoint_quat, + "ee_linvel": self.fingertip_midpoint_linvel, + "ee_angvel": self.fingertip_midpoint_angvel, + "joint_pos": self.joint_pos[:, 0:7], + "held_pos": self.held_pos, + "held_pos_rel_fixed": self.held_pos - self.fixed_pos_obs_frame, + "held_quat": self.held_quat, + "fixed_pos": self.fixed_pos, + "fixed_quat": self.fixed_quat, + "task_prop_gains": self.task_prop_gains, + "pos_threshold": self.pos_threshold, + "rot_threshold": self.rot_threshold, + "prev_actions": prev_actions, + } + obs_tensors = [obs_dict[obs_name] for obs_name in self.cfg.obs_order + ["prev_actions"]] + obs_tensors = torch.cat(obs_tensors, dim=-1) + state_tensors = [state_dict[state_name] for state_name in self.cfg.state_order + ["prev_actions"]] + state_tensors = torch.cat(state_tensors, dim=-1) + return {"policy": obs_tensors, "critic": state_tensors} + + def _reset_buffers(self, env_ids): + """Reset buffers.""" + self.ep_succeeded[env_ids] = 0 + + def _pre_physics_step(self, action): + """Apply policy actions with smoothing.""" + env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if len(env_ids) > 0: + self._reset_buffers(env_ids) + + self.actions = ( + self.cfg.ctrl.ema_factor * action.clone().to(self.device) + (1 - self.cfg.ctrl.ema_factor) * self.actions + ) + + def close_gripper_in_place(self): + """Keep gripper in current position as gripper closes.""" + actions = torch.zeros((self.num_envs, 6), device=self.device) + ctrl_target_gripper_dof_pos = 0.0 + + # Interpret actions as target pos displacements and set pos target + pos_actions = actions[:, 0:3] * self.pos_threshold + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = actions[:, 3:6] + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1.0e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = ctrl_target_gripper_dof_pos + self.generate_ctrl_signals() + + def _apply_action(self): + """Apply actions for policy as delta targets from current position.""" + # Get current yaw for success checking. + _, _, curr_yaw = torch_utils.get_euler_xyz(self.fingertip_midpoint_quat) + self.curr_yaw = torch.where(curr_yaw > np.deg2rad(235), curr_yaw - 2 * np.pi, curr_yaw) + + # Note: We use finite-differenced velocities for control and observations. + # Check if we need to re-compute velocities within the decimation loop. + if self.last_update_timestamp < self._robot._data._sim_timestamp: + self._compute_intermediate_values(dt=self.physics_dt) + + # Interpret actions as target pos displacements and set pos target + pos_actions = self.actions[:, 0:3] * self.pos_threshold + + # Interpret actions as target rot (axis-angle) displacements + rot_actions = self.actions[:, 3:6] + if self.cfg_task.unidirectional_rot: + rot_actions[:, 2] = -(rot_actions[:, 2] + 1.0) * 0.5 # [-1, 0] + rot_actions = rot_actions * self.rot_threshold + + self.ctrl_target_fingertip_midpoint_pos = self.fingertip_midpoint_pos + pos_actions + # To speed up learning, never allow the policy to move more than 5cm away from the base. + delta_pos = self.ctrl_target_fingertip_midpoint_pos - self.fixed_pos_action_frame + pos_error_clipped = torch.clip( + delta_pos, -self.cfg.ctrl.pos_action_bounds[0], self.cfg.ctrl.pos_action_bounds[1] + ) + self.ctrl_target_fingertip_midpoint_pos = self.fixed_pos_action_frame + pos_error_clipped + + # Convert to quat and set rot target + angle = torch.norm(rot_actions, p=2, dim=-1) + axis = rot_actions / angle.unsqueeze(-1) + + rot_actions_quat = torch_utils.quat_from_angle_axis(angle, axis) + rot_actions_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > 1e-6, + rot_actions_quat, + torch.tensor([1.0, 0.0, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1), + ) + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_mul(rot_actions_quat, self.fingertip_midpoint_quat) + + target_euler_xyz = torch.stack(torch_utils.get_euler_xyz(self.ctrl_target_fingertip_midpoint_quat), dim=1) + target_euler_xyz[:, 0] = 3.14159 # Restrict actions to be upright. + target_euler_xyz[:, 1] = 0.0 + + self.ctrl_target_fingertip_midpoint_quat = torch_utils.quat_from_euler_xyz( + roll=target_euler_xyz[:, 0], pitch=target_euler_xyz[:, 1], yaw=target_euler_xyz[:, 2] + ) + + self.ctrl_target_gripper_dof_pos = 0.0 + self.generate_ctrl_signals() + + def _set_gains(self, prop_gains, rot_deriv_scale=1.0): + """Set robot gains using critical damping.""" + self.task_prop_gains = prop_gains + self.task_deriv_gains = 2 * torch.sqrt(prop_gains) + self.task_deriv_gains[:, 3:6] /= rot_deriv_scale + + def generate_ctrl_signals(self): + """Get Jacobian. Set Franka DOF position targets (fingers) or DOF torques (arm).""" + self.joint_torque, self.applied_wrench = fc.compute_dof_torque( + cfg=self.cfg, + dof_pos=self.joint_pos, + dof_vel=self.joint_vel, # _fd, + fingertip_midpoint_pos=self.fingertip_midpoint_pos, + fingertip_midpoint_quat=self.fingertip_midpoint_quat, + fingertip_midpoint_linvel=self.ee_linvel_fd, + fingertip_midpoint_angvel=self.ee_angvel_fd, + jacobian=self.fingertip_midpoint_jacobian, + arm_mass_matrix=self.arm_mass_matrix, + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos, + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat, + task_prop_gains=self.task_prop_gains, + task_deriv_gains=self.task_deriv_gains, + device=self.device, + ) + + # set target for gripper joints to use physx's PD controller + self.ctrl_target_joint_pos[:, 7:9] = self.ctrl_target_gripper_dof_pos + self.joint_torque[:, 7:9] = 0.0 + + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + self._robot.set_joint_effort_target(self.joint_torque) + + def _get_dones(self): + """Update intermediate values used for rewards and observations.""" + self._compute_intermediate_values(dt=self.physics_dt) + time_out = self.episode_length_buf >= self.max_episode_length - 1 + return time_out, time_out + + def _get_curr_successes(self, success_threshold, check_rot=False): + """Get success mask at current timestep.""" + curr_successes = torch.zeros((self.num_envs,), dtype=torch.bool, device=self.device) + + xy_dist = torch.linalg.vector_norm(self.target_held_base_pos[:, 0:2] - self.held_base_pos[:, 0:2], dim=1) + z_disp = self.held_base_pos[:, 2] - self.target_held_base_pos[:, 2] + + is_centered = torch.where(xy_dist < 0.0025, torch.ones_like(curr_successes), torch.zeros_like(curr_successes)) + # Height threshold to target + fixed_cfg = self.cfg_task.fixed_asset_cfg + if self.cfg_task.name == "peg_insert" or self.cfg_task.name == "gear_mesh": + height_threshold = fixed_cfg.height * success_threshold + elif self.cfg_task.name == "nut_thread": + height_threshold = fixed_cfg.thread_pitch * success_threshold + else: + raise NotImplementedError("Task not implemented") + is_close_or_below = torch.where( + z_disp < height_threshold, torch.ones_like(curr_successes), torch.zeros_like(curr_successes) + ) + curr_successes = torch.logical_and(is_centered, is_close_or_below) + + if check_rot: + is_rotated = self.curr_yaw < self.cfg_task.ee_success_yaw + curr_successes = torch.logical_and(curr_successes, is_rotated) + + return curr_successes + + def _get_rewards(self): + """Update rewards and compute success statistics.""" + # Get successful and failed envs at current timestep + check_rot = self.cfg_task.name == "nut_thread" + curr_successes = self._get_curr_successes( + success_threshold=self.cfg_task.success_threshold, check_rot=check_rot + ) + + rew_buf = self._update_rew_buf(curr_successes) + + # Only log episode success rates at the end of an episode. + if torch.any(self.reset_buf): + self.extras["successes"] = torch.count_nonzero(curr_successes) / self.num_envs + + # Get the time at which an episode first succeeds. + first_success = torch.logical_and(curr_successes, torch.logical_not(self.ep_succeeded)) + self.ep_succeeded[curr_successes] = 1 + + first_success_ids = first_success.nonzero(as_tuple=False).squeeze(-1) + self.ep_success_times[first_success_ids] = self.episode_length_buf[first_success_ids] + nonzero_success_ids = self.ep_success_times.nonzero(as_tuple=False).squeeze(-1) + + if len(nonzero_success_ids) > 0: # Only log for successful episodes. + success_times = self.ep_success_times[nonzero_success_ids].sum() / len(nonzero_success_ids) + self.extras["success_times"] = success_times + + self.prev_actions = self.actions.clone() + return rew_buf + + def _update_rew_buf(self, curr_successes): + """Compute reward at current timestep.""" + rew_dict = {} + + # Keypoint rewards. + def squashing_fn(x, a, b): + return 1 / (torch.exp(a * x) + b + torch.exp(-a * x)) + + a0, b0 = self.cfg_task.keypoint_coef_baseline + rew_dict["kp_baseline"] = squashing_fn(self.keypoint_dist, a0, b0) + # a1, b1 = 25, 2 + a1, b1 = self.cfg_task.keypoint_coef_coarse + rew_dict["kp_coarse"] = squashing_fn(self.keypoint_dist, a1, b1) + a2, b2 = self.cfg_task.keypoint_coef_fine + # a2, b2 = 300, 0 + rew_dict["kp_fine"] = squashing_fn(self.keypoint_dist, a2, b2) + + # Action penalties. + rew_dict["action_penalty"] = torch.norm(self.actions, p=2) + rew_dict["action_grad_penalty"] = torch.norm(self.actions - self.prev_actions, p=2, dim=-1) + rew_dict["curr_engaged"] = ( + self._get_curr_successes(success_threshold=self.cfg_task.engage_threshold, check_rot=False).clone().float() + ) + rew_dict["curr_successes"] = curr_successes.clone().float() + + rew_buf = ( + rew_dict["kp_coarse"] + + rew_dict["kp_baseline"] + + rew_dict["kp_fine"] + - rew_dict["action_penalty"] * self.cfg_task.action_penalty_scale + - rew_dict["action_grad_penalty"] * self.cfg_task.action_grad_penalty_scale + + rew_dict["curr_engaged"] + + rew_dict["curr_successes"] + ) + + for rew_name, rew in rew_dict.items(): + self.extras[f"logs_rew_{rew_name}"] = rew.mean() + + return rew_buf + + def _reset_idx(self, env_ids): + """ + We assume all envs will always be reset at the same time. + """ + super()._reset_idx(env_ids) + + self._set_assets_to_default_pose(env_ids) + self._set_franka_to_default_pose(joints=self.cfg.ctrl.reset_joints, env_ids=env_ids) + self.step_sim_no_action() + + self.randomize_initial_state(env_ids) + + def _get_target_gear_base_offset(self): + """Get offset of target gear from the gear base asset.""" + target_gear = self.cfg_task.target_gear + if target_gear == "gear_large": + gear_base_offset = self.cfg_task.fixed_asset_cfg.large_gear_base_offset + elif target_gear == "gear_medium": + gear_base_offset = self.cfg_task.fixed_asset_cfg.medium_gear_base_offset + elif target_gear == "gear_small": + gear_base_offset = self.cfg_task.fixed_asset_cfg.small_gear_base_offset + else: + raise ValueError(f"{target_gear} not valid in this context!") + return gear_base_offset + + def _set_assets_to_default_pose(self, env_ids): + """Move assets to default pose before randomization.""" + held_state = self._held_asset.data.default_root_state.clone()[env_ids] + held_state[:, 0:3] += self.scene.env_origins[env_ids] + held_state[:, 7:] = 0.0 + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7], env_ids=env_ids) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:], env_ids=env_ids) + self._held_asset.reset() + + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + fixed_state[:, 0:3] += self.scene.env_origins[env_ids] + fixed_state[:, 7:] = 0.0 + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.reset() + + def set_pos_inverse_kinematics(self, env_ids): + """Set robot joint position using DLS IK.""" + ik_time = 0.0 + while ik_time < 0.25: + # Compute error to target. + pos_error, axis_angle_error = fc.get_pose_error( + fingertip_midpoint_pos=self.fingertip_midpoint_pos[env_ids], + fingertip_midpoint_quat=self.fingertip_midpoint_quat[env_ids], + ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_midpoint_pos[env_ids], + ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_midpoint_quat[env_ids], + jacobian_type="geometric", + rot_error_type="axis_angle", + ) + + delta_hand_pose = torch.cat((pos_error, axis_angle_error), dim=-1) + + # Solve DLS problem. + delta_dof_pos = fc._get_delta_dof_pos( + delta_pose=delta_hand_pose, + ik_method="dls", + jacobian=self.fingertip_midpoint_jacobian[env_ids], + device=self.device, + ) + self.joint_pos[env_ids, 0:7] += delta_dof_pos[:, 0:7] + self.joint_vel[env_ids, :] = torch.zeros_like(self.joint_pos[env_ids,]) + + self.ctrl_target_joint_pos[env_ids, 0:7] = self.joint_pos[env_ids, 0:7] + # Update dof state. + self._robot.write_joint_state_to_sim(self.joint_pos, self.joint_vel) + self._robot.set_joint_position_target(self.ctrl_target_joint_pos) + + # Simulate and update tensors. + self.step_sim_no_action() + ik_time += self.physics_dt + + return pos_error, axis_angle_error + + def get_handheld_asset_relative_pose(self): + """Get default relative pose between help asset and fingertip.""" + if self.cfg_task.name == "peg_insert": + held_asset_relative_pos = torch.zeros_like(self.held_base_pos_local) + held_asset_relative_pos[:, 2] = self.cfg_task.held_asset_cfg.height + held_asset_relative_pos[:, 2] -= self.cfg_task.robot_cfg.franka_fingerpad_length + elif self.cfg_task.name == "gear_mesh": + held_asset_relative_pos = torch.zeros_like(self.held_base_pos_local) + gear_base_offset = self._get_target_gear_base_offset() + held_asset_relative_pos[:, 0] += gear_base_offset[0] + held_asset_relative_pos[:, 2] += gear_base_offset[2] + held_asset_relative_pos[:, 2] += self.cfg_task.held_asset_cfg.height / 2.0 * 1.1 + elif self.cfg_task.name == "nut_thread": + held_asset_relative_pos = self.held_base_pos_local + else: + raise NotImplementedError("Task not implemented") + + held_asset_relative_quat = self.identity_quat + if self.cfg_task.name == "nut_thread": + # Rotate along z-axis of frame for default position. + initial_rot_deg = self.cfg_task.held_asset_rot_init + rot_yaw_euler = torch.tensor([0.0, 0.0, initial_rot_deg * np.pi / 180.0], device=self.device).repeat( + self.num_envs, 1 + ) + held_asset_relative_quat = torch_utils.quat_from_euler_xyz( + roll=rot_yaw_euler[:, 0], pitch=rot_yaw_euler[:, 1], yaw=rot_yaw_euler[:, 2] + ) + + return held_asset_relative_pos, held_asset_relative_quat + + def _set_franka_to_default_pose(self, joints, env_ids): + """Return Franka to its default joint position.""" + gripper_width = self.cfg_task.held_asset_cfg.diameter / 2 * 1.25 + joint_pos = self._robot.data.default_joint_pos[env_ids] + joint_pos[:, 7:] = gripper_width # MIMIC + joint_pos[:, :7] = torch.tensor(joints, device=self.device)[None, :] + joint_vel = torch.zeros_like(joint_pos) + joint_effort = torch.zeros_like(joint_pos) + self.ctrl_target_joint_pos[env_ids, :] = joint_pos + self._robot.set_joint_position_target(self.ctrl_target_joint_pos[env_ids], env_ids=env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + self._robot.reset() + self._robot.set_joint_effort_target(joint_effort, env_ids=env_ids) + + self.step_sim_no_action() + + def step_sim_no_action(self): + """Step the simulation without an action. Used for resets.""" + self.scene.write_data_to_sim() + self.sim.step(render=False) + self.scene.update(dt=self.physics_dt) + self._compute_intermediate_values(dt=self.physics_dt) + + def randomize_initial_state(self, env_ids): + """Randomize initial state and perform any episode-level randomization.""" + # Disable gravity. + physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) + + # (1.) Randomize fixed asset pose. + fixed_state = self._fixed_asset.data.default_root_state.clone()[env_ids] + # (1.a.) Position + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + fixed_asset_init_pos_rand = torch.tensor( + self.cfg_task.fixed_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + fixed_pos_init_rand = fixed_pos_init_rand @ torch.diag(fixed_asset_init_pos_rand) + fixed_state[:, 0:3] += fixed_pos_init_rand + self.scene.env_origins[env_ids] + # (1.b.) Orientation + fixed_orn_init_yaw = np.deg2rad(self.cfg_task.fixed_asset_init_orn_deg) + fixed_orn_yaw_range = np.deg2rad(self.cfg_task.fixed_asset_init_orn_range_deg) + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_orn_euler = fixed_orn_init_yaw + fixed_orn_yaw_range * rand_sample + fixed_orn_euler[:, 0:2] = 0.0 # Only change yaw. + fixed_orn_quat = torch_utils.quat_from_euler_xyz( + fixed_orn_euler[:, 0], fixed_orn_euler[:, 1], fixed_orn_euler[:, 2] + ) + fixed_state[:, 3:7] = fixed_orn_quat + # (1.c.) Velocity + fixed_state[:, 7:] = 0.0 # vel + # (1.d.) Update values. + self._fixed_asset.write_root_pose_to_sim(fixed_state[:, 0:7], env_ids=env_ids) + self._fixed_asset.write_root_velocity_to_sim(fixed_state[:, 7:], env_ids=env_ids) + self._fixed_asset.reset() + + # (1.e.) Noisy position observation. + fixed_asset_pos_noise = torch.randn((len(env_ids), 3), dtype=torch.float32, device=self.device) + fixed_asset_pos_rand = torch.tensor(self.cfg.obs_rand.fixed_asset_pos, dtype=torch.float32, device=self.device) + fixed_asset_pos_noise = fixed_asset_pos_noise @ torch.diag(fixed_asset_pos_rand) + self.init_fixed_pos_obs_noise[:] = fixed_asset_pos_noise + + self.step_sim_no_action() + + # Compute the frame on the bolt that would be used as observation: fixed_pos_obs_frame + # For example, the tip of the bolt can be used as the observation frame + fixed_tip_pos_local = torch.zeros_like(self.fixed_pos) + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.height + fixed_tip_pos_local[:, 2] += self.cfg_task.fixed_asset_cfg.base_height + if self.cfg_task.name == "gear_mesh": + fixed_tip_pos_local[:, 0] = self._get_target_gear_base_offset()[0] + + _, fixed_tip_pos = torch_utils.tf_combine( + self.fixed_quat, self.fixed_pos, self.identity_quat, fixed_tip_pos_local + ) + self.fixed_pos_obs_frame[:] = fixed_tip_pos + + # (2) Move gripper to randomizes location above fixed asset. Keep trying until IK succeeds. + # (a) get position vector to target + bad_envs = env_ids.clone() + ik_attempt = 0 + + hand_down_quat = torch.zeros((self.num_envs, 4), dtype=torch.float32, device=self.device) + self.hand_down_euler = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device) + while True: + n_bad = bad_envs.shape[0] + + above_fixed_pos = fixed_tip_pos.clone() + above_fixed_pos[:, 2] += self.cfg_task.hand_init_pos[2] + + rand_sample = torch.rand((n_bad, 3), dtype=torch.float32, device=self.device) + above_fixed_pos_rand = 2 * (rand_sample - 0.5) # [-1, 1] + hand_init_pos_rand = torch.tensor(self.cfg_task.hand_init_pos_noise, device=self.device) + above_fixed_pos_rand = above_fixed_pos_rand @ torch.diag(hand_init_pos_rand) + above_fixed_pos[bad_envs] += above_fixed_pos_rand + + # (b) get random orientation facing down + hand_down_euler = ( + torch.tensor(self.cfg_task.hand_init_orn, device=self.device).unsqueeze(0).repeat(n_bad, 1) + ) + + rand_sample = torch.rand((n_bad, 3), dtype=torch.float32, device=self.device) + above_fixed_orn_noise = 2 * (rand_sample - 0.5) # [-1, 1] + hand_init_orn_rand = torch.tensor(self.cfg_task.hand_init_orn_noise, device=self.device) + above_fixed_orn_noise = above_fixed_orn_noise @ torch.diag(hand_init_orn_rand) + hand_down_euler += above_fixed_orn_noise + self.hand_down_euler[bad_envs, ...] = hand_down_euler + hand_down_quat[bad_envs, :] = torch_utils.quat_from_euler_xyz( + roll=hand_down_euler[:, 0], pitch=hand_down_euler[:, 1], yaw=hand_down_euler[:, 2] + ) + + # (c) iterative IK Method + self.ctrl_target_fingertip_midpoint_pos[bad_envs, ...] = above_fixed_pos[bad_envs, ...] + self.ctrl_target_fingertip_midpoint_quat[bad_envs, ...] = hand_down_quat[bad_envs, :] + + pos_error, aa_error = self.set_pos_inverse_kinematics(env_ids=bad_envs) + pos_error = torch.linalg.norm(pos_error, dim=1) > 1e-3 + angle_error = torch.norm(aa_error, dim=1) > 1e-3 + any_error = torch.logical_or(pos_error, angle_error) + bad_envs = bad_envs[any_error.nonzero(as_tuple=False).squeeze(-1)] + + # Check IK succeeded for all envs, otherwise try again for those envs + if bad_envs.shape[0] == 0: + break + + self._set_franka_to_default_pose( + joints=[0.00871, -0.10368, -0.00794, -1.49139, -0.00083, 1.38774, 0.0], env_ids=bad_envs + ) + + ik_attempt += 1 + + self.step_sim_no_action() + + # Add flanking gears after servo (so arm doesn't move them). + if self.cfg_task.name == "gear_mesh" and self.cfg_task.add_flanking_gears: + small_gear_state = self._small_gear_asset.data.default_root_state.clone()[env_ids] + small_gear_state[:, 0:7] = fixed_state[:, 0:7] + small_gear_state[:, 7:] = 0.0 # vel + self._small_gear_asset.write_root_pose_to_sim(small_gear_state[:, 0:7], env_ids=env_ids) + self._small_gear_asset.write_root_velocity_to_sim(small_gear_state[:, 7:], env_ids=env_ids) + self._small_gear_asset.reset() + + large_gear_state = self._large_gear_asset.data.default_root_state.clone()[env_ids] + large_gear_state[:, 0:7] = fixed_state[:, 0:7] + large_gear_state[:, 7:] = 0.0 # vel + self._large_gear_asset.write_root_pose_to_sim(large_gear_state[:, 0:7], env_ids=env_ids) + self._large_gear_asset.write_root_velocity_to_sim(large_gear_state[:, 7:], env_ids=env_ids) + self._large_gear_asset.reset() + + # (3) Randomize asset-in-gripper location. + # flip gripper z orientation + flip_z_quat = torch.tensor([0.0, 0.0, 1.0, 0.0], device=self.device).unsqueeze(0).repeat(self.num_envs, 1) + fingertip_flipped_quat, fingertip_flipped_pos = torch_utils.tf_combine( + q1=self.fingertip_midpoint_quat, + t1=self.fingertip_midpoint_pos, + q2=flip_z_quat, + t2=torch.zeros_like(self.fingertip_midpoint_pos), + ) + + # get default gripper in asset transform + held_asset_relative_pos, held_asset_relative_quat = self.get_handheld_asset_relative_pose() + asset_in_hand_quat, asset_in_hand_pos = torch_utils.tf_inverse( + held_asset_relative_quat, held_asset_relative_pos + ) + + translated_held_asset_quat, translated_held_asset_pos = torch_utils.tf_combine( + q1=fingertip_flipped_quat, t1=fingertip_flipped_pos, q2=asset_in_hand_quat, t2=asset_in_hand_pos + ) + + # Add asset in hand randomization + rand_sample = torch.rand((self.num_envs, 3), dtype=torch.float32, device=self.device) + self.held_asset_pos_noise = 2 * (rand_sample - 0.5) # [-1, 1] + if self.cfg_task.name == "gear_mesh": + self.held_asset_pos_noise[:, 2] = -rand_sample[:, 2] # [-1, 0] + + held_asset_pos_noise = torch.tensor(self.cfg_task.held_asset_pos_noise, device=self.device) + self.held_asset_pos_noise = self.held_asset_pos_noise @ torch.diag(held_asset_pos_noise) + translated_held_asset_quat, translated_held_asset_pos = torch_utils.tf_combine( + q1=translated_held_asset_quat, + t1=translated_held_asset_pos, + q2=self.identity_quat, + t2=self.held_asset_pos_noise, + ) + + held_state = self._held_asset.data.default_root_state.clone() + held_state[:, 0:3] = translated_held_asset_pos + self.scene.env_origins + held_state[:, 3:7] = translated_held_asset_quat + held_state[:, 7:] = 0.0 + self._held_asset.write_root_pose_to_sim(held_state[:, 0:7]) + self._held_asset.write_root_velocity_to_sim(held_state[:, 7:]) + self._held_asset.reset() + + # Close hand + # Set gains to use for quick resets. + reset_task_prop_gains = torch.tensor(self.cfg.ctrl.reset_task_prop_gains, device=self.device).repeat( + (self.num_envs, 1) + ) + reset_rot_deriv_scale = self.cfg.ctrl.reset_rot_deriv_scale + self._set_gains(reset_task_prop_gains, reset_rot_deriv_scale) + + self.step_sim_no_action() + + grasp_time = 0.0 + while grasp_time < 0.25: + self.ctrl_target_joint_pos[env_ids, 7:] = 0.0 # Close gripper. + self.ctrl_target_gripper_dof_pos = 0.0 + self.close_gripper_in_place() + self.step_sim_no_action() + grasp_time += self.sim.get_physics_dt() + + self.prev_joint_pos = self.joint_pos[:, 0:7].clone() + self.prev_fingertip_pos = self.fingertip_midpoint_pos.clone() + self.prev_fingertip_quat = self.fingertip_midpoint_quat.clone() + + # Set initial actions to involve no-movement. Needed for EMA/correct penalties. + self.actions = torch.zeros_like(self.actions) + self.prev_actions = torch.zeros_like(self.actions) + # Back out what actions should be for initial state. + # Relative position to bolt tip. + self.fixed_pos_action_frame[:] = self.fixed_pos_obs_frame + self.init_fixed_pos_obs_noise + + pos_actions = self.fingertip_midpoint_pos - self.fixed_pos_action_frame + pos_action_bounds = torch.tensor(self.cfg.ctrl.pos_action_bounds, device=self.device) + pos_actions = pos_actions @ torch.diag(1.0 / pos_action_bounds) + self.actions[:, 0:3] = self.prev_actions[:, 0:3] = pos_actions + + # Relative yaw to bolt. + unrot_180_euler = torch.tensor([-np.pi, 0.0, 0.0], device=self.device).repeat(self.num_envs, 1) + unrot_quat = torch_utils.quat_from_euler_xyz( + roll=unrot_180_euler[:, 0], pitch=unrot_180_euler[:, 1], yaw=unrot_180_euler[:, 2] + ) + + fingertip_quat_rel_bolt = torch_utils.quat_mul(unrot_quat, self.fingertip_midpoint_quat) + fingertip_yaw_bolt = torch_utils.get_euler_xyz(fingertip_quat_rel_bolt)[-1] + fingertip_yaw_bolt = torch.where( + fingertip_yaw_bolt > torch.pi / 2, fingertip_yaw_bolt - 2 * torch.pi, fingertip_yaw_bolt + ) + fingertip_yaw_bolt = torch.where( + fingertip_yaw_bolt < -torch.pi, fingertip_yaw_bolt + 2 * torch.pi, fingertip_yaw_bolt + ) + + yaw_action = (fingertip_yaw_bolt + np.deg2rad(180.0)) / np.deg2rad(270.0) * 2.0 - 1.0 + self.actions[:, 5] = self.prev_actions[:, 5] = yaw_action + + # Zero initial velocity. + self.ee_angvel_fd[:, :] = 0.0 + self.ee_linvel_fd[:, :] = 0.0 + + # Set initial gains for the episode. + self._set_gains(self.default_gains) + + physics_sim_view.set_gravity(carb.Float3(*self.cfg.sim.gravity)) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env_cfg.py new file mode 100644 index 00000000000..9358174c3b8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -0,0 +1,213 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass + +from .factory_tasks_cfg import ASSET_DIR, FactoryTask, GearMesh, NutThread, PegInsert + +OBS_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, +} + +STATE_DIM_CFG = { + "fingertip_pos": 3, + "fingertip_pos_rel_fixed": 3, + "fingertip_quat": 4, + "ee_linvel": 3, + "ee_angvel": 3, + "joint_pos": 7, + "held_pos": 3, + "held_pos_rel_fixed": 3, + "held_quat": 4, + "fixed_pos": 3, + "fixed_quat": 4, + "task_prop_gains": 6, + "ema_factor": 1, + "pos_threshold": 3, + "rot_threshold": 3, +} + + +@configclass +class ObsRandCfg: + fixed_asset_pos = [0.001, 0.001, 0.001] + + +@configclass +class CtrlCfg: + ema_factor = 0.2 + + pos_action_bounds = [0.05, 0.05, 0.05] + rot_action_bounds = [1.0, 1.0, 1.0] + + pos_action_threshold = [0.02, 0.02, 0.02] + rot_action_threshold = [0.097, 0.097, 0.097] + + reset_joints = [1.5178e-03, -1.9651e-01, -1.4364e-03, -1.9761, -2.7717e-04, 1.7796, 7.8556e-01] + reset_task_prop_gains = [300, 300, 300, 20, 20, 20] + reset_rot_deriv_scale = 10.0 + default_task_prop_gains = [100, 100, 100, 30, 30, 30] + + # Null space parameters. + default_dof_pos_tensor = [-1.3003, -0.4015, 1.1791, -2.1493, 0.4001, 1.9425, 0.4754] + kp_null = 10.0 + kd_null = 6.3246 + + +@configclass +class FactoryEnvCfg(DirectRLEnvCfg): + decimation = 8 + action_space = 6 + # num_*: will be overwritten to correspond to obs_order, state_order. + observation_space = 21 + state_space = 72 + obs_order: list = ["fingertip_pos_rel_fixed", "fingertip_quat", "ee_linvel", "ee_angvel"] + state_order: list = [ + "fingertip_pos", + "fingertip_quat", + "ee_linvel", + "ee_angvel", + "joint_pos", + "held_pos", + "held_pos_rel_fixed", + "held_quat", + "fixed_pos", + "fixed_quat", + ] + + task_name: str = "peg_insert" # peg_insert, gear_mesh, nut_thread + task: FactoryTask = FactoryTask() + obs_rand: ObsRandCfg = ObsRandCfg() + ctrl: CtrlCfg = CtrlCfg() + + episode_length_s = 10.0 # Probably need to override. + sim: SimulationCfg = SimulationCfg( + device="cuda:0", + dt=1 / 120, + gravity=(0.0, 0.0, -9.81), + physx=PhysxCfg( + solver_type=1, + max_position_iteration_count=192, # Important to avoid interpenetration. + max_velocity_iteration_count=1, + bounce_threshold_velocity=0.2, + friction_offset_threshold=0.01, + friction_correlation_distance=0.00625, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + gpu_max_num_partitions=1, # Important for stable simulation. + ), + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + ) + + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0) + + robot = ArticulationCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ASSET_DIR}/franka_mimic.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.00871, + "panda_joint2": -0.10368, + "panda_joint3": -0.00794, + "panda_joint4": -1.49139, + "panda_joint5": -0.00083, + "panda_joint6": 1.38774, + "panda_joint7": 0.0, + "panda_finger_joint2": 0.04, + }, + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + ), + actuators={ + "panda_arm1": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=87, + velocity_limit=124.6, + ), + "panda_arm2": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + stiffness=0.0, + damping=0.0, + friction=0.0, + armature=0.0, + effort_limit=12, + velocity_limit=149.5, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint[1-2]"], + effort_limit=40.0, + velocity_limit=0.04, + stiffness=7500.0, + damping=173.0, + friction=0.1, + armature=0.0, + ), + }, + ) + + +@configclass +class FactoryTaskPegInsertCfg(FactoryEnvCfg): + task_name = "peg_insert" + task = PegInsert() + episode_length_s = 10.0 + + +@configclass +class FactoryTaskGearMeshCfg(FactoryEnvCfg): + task_name = "gear_mesh" + task = GearMesh() + episode_length_s = 20.0 + + +@configclass +class FactoryTaskNutThreadCfg(FactoryEnvCfg): + task_name = "nut_thread" + task = NutThread() + episode_length_s = 30.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_tasks_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_tasks_cfg.py new file mode 100644 index 00000000000..23cf9d810f9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/factory/factory_tasks_cfg.py @@ -0,0 +1,453 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +ASSET_DIR = f"{ISAACLAB_NUCLEUS_DIR}/Factory" + + +@configclass +class FixedAssetCfg: + usd_path: str = "" + diameter: float = 0.0 + height: float = 0.0 + base_height: float = 0.0 # Used to compute held asset CoM. + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class HeldAssetCfg: + usd_path: str = "" + diameter: float = 0.0 # Used for gripper width. + height: float = 0.0 + friction: float = 0.75 + mass: float = 0.05 + + +@configclass +class RobotCfg: + robot_usd: str = "" + franka_fingerpad_length: float = 0.017608 + friction: float = 0.75 + + +@configclass +class FactoryTask: + robot_cfg: RobotCfg = RobotCfg() + name: str = "" + duration_s = 5.0 + + fixed_asset_cfg: FixedAssetCfg = FixedAssetCfg() + held_asset_cfg: HeldAssetCfg = HeldAssetCfg() + asset_size: float = 0.0 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.015] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0, 2.356] + hand_init_orn_noise: list = [0.0, 0.0, 1.57] + + # Action + unidirectional_rot: bool = False + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 360.0 + + # Held Asset (applies to all tasks) + held_asset_pos_noise: list = [0.0, 0.006, 0.003] # noise level of the held asset in gripper + held_asset_rot_init: float = -90.0 + + # Reward + ee_success_yaw: float = 0.0 # nut_thread task only. + action_penalty_scale: float = 0.0 + action_grad_penalty_scale: float = 0.0 + # Reward function details can be found in Appendix B of https://arxiv.org/pdf/2408.04587. + # Multi-scale keypoints are used to capture different phases of the task. + # Each reward passes the keypoint distance, x, through a squashing function: + # r(x) = 1/(exp(-ax) + b + exp(ax)). + # Each list defines [a, b] which control the slope and maximum of the squashing function. + num_keypoints: int = 4 + keypoint_scale: float = 0.15 + keypoint_coef_baseline: list = [5, 4] # General movement towards fixed object. + keypoint_coef_coarse: list = [50, 2] # Movement to align the assets. + keypoint_coef_fine: list = [100, 0] # Smaller distances for threading or last-inch insertion. + # Fixed-asset height fraction for which different bonuses are rewarded (see individual tasks). + success_threshold: float = 0.04 + engage_threshold: float = 0.9 + + +@configclass +class Peg8mm(HeldAssetCfg): + usd_path = f"{ASSET_DIR}/factory_peg_8mm.usd" + diameter = 0.007986 + height = 0.050 + mass = 0.019 + + +@configclass +class Hole8mm(FixedAssetCfg): + usd_path = f"{ASSET_DIR}/factory_hole_8mm.usd" + diameter = 0.0081 + height = 0.025 + base_height = 0.0 + + +@configclass +class PegInsert(FactoryTask): + name = "peg_insert" + fixed_asset_cfg = Hole8mm() + held_asset_cfg = Peg8mm() + asset_size = 8.0 + duration_s = 10.0 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.047] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0.0, 0.0] + hand_init_orn_noise: list = [0.0, 0.0, 0.785] + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 360.0 + + # Held Asset (applies to all tasks) + held_asset_pos_noise: list = [0.003, 0.0, 0.003] # noise level of the held asset in gripper + held_asset_rot_init: float = 0.0 + + # Rewards + keypoint_coef_baseline: list = [5, 4] + keypoint_coef_coarse: list = [50, 2] + keypoint_coef_fine: list = [100, 0] + # Fraction of socket height. + success_threshold: float = 0.04 + engage_threshold: float = 0.9 + + fixed_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/FixedAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=fixed_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=fixed_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.6, 0.0, 0.05), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + held_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/HeldAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=held_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=held_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + + +@configclass +class GearBase(FixedAssetCfg): + usd_path = f"{ASSET_DIR}/factory_gear_base.usd" + height = 0.02 + base_height = 0.005 + small_gear_base_offset = [5.075e-2, 0.0, 0.0] + medium_gear_base_offset = [2.025e-2, 0.0, 0.0] + large_gear_base_offset = [-3.025e-2, 0.0, 0.0] + + +@configclass +class MediumGear(HeldAssetCfg): + usd_path = f"{ASSET_DIR}/factory_gear_medium.usd" + diameter = 0.03 # Used for gripper width. + height: float = 0.03 + mass = 0.012 + + +@configclass +class GearMesh(FactoryTask): + name = "gear_mesh" + fixed_asset_cfg = GearBase() + held_asset_cfg = MediumGear() + target_gear = "gear_medium" + duration_s = 20.0 + + small_gear_usd = f"{ASSET_DIR}/factory_gear_small.usd" + large_gear_usd = f"{ASSET_DIR}/factory_gear_large.usd" + + small_gear_cfg: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/SmallGearAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=small_gear_usd, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.019), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + + large_gear_cfg: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/LargeGearAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=large_gear_usd, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.019), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + + # Gears Asset + add_flanking_gears = True + add_flanking_gears_prob = 1.0 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.035] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0, 0.0] + hand_init_orn_noise: list = [0.0, 0.0, 0.785] + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 0.0 + fixed_asset_init_orn_range_deg: float = 15.0 + + # Held Asset (applies to all tasks) + held_asset_pos_noise: list = [0.003, 0.0, 0.003] # noise level of the held asset in gripper + held_asset_rot_init: float = -90.0 + + keypoint_coef_baseline: list = [5, 4] + keypoint_coef_coarse: list = [50, 2] + keypoint_coef_fine: list = [100, 0] + # Fraction of gear peg height. + success_threshold: float = 0.05 + engage_threshold: float = 0.9 + + fixed_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/FixedAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=fixed_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=fixed_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.6, 0.0, 0.05), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + held_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/HeldAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=held_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=held_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + + +@configclass +class NutM16(HeldAssetCfg): + usd_path = f"{ASSET_DIR}/factory_nut_m16.usd" + diameter = 0.024 + height = 0.01 + mass = 0.03 + friction = 0.01 # Additive with the nut means friction is (-0.25 + 0.75)/2 = 0.25 + + +@configclass +class BoltM16(FixedAssetCfg): + usd_path = f"{ASSET_DIR}/factory_bolt_m16.usd" + diameter = 0.024 + height = 0.025 + base_height = 0.01 + thread_pitch = 0.002 + + +@configclass +class NutThread(FactoryTask): + name = "nut_thread" + fixed_asset_cfg = BoltM16() + held_asset_cfg = NutM16() + asset_size = 16.0 + duration_s = 30.0 + + # Robot + hand_init_pos: list = [0.0, 0.0, 0.015] # Relative to fixed asset tip. + hand_init_pos_noise: list = [0.02, 0.02, 0.01] + hand_init_orn: list = [3.1416, 0.0, 1.83] + hand_init_orn_noise: list = [0.0, 0.0, 0.26] + + # Action + unidirectional_rot: bool = True + + # Fixed Asset (applies to all tasks) + fixed_asset_init_pos_noise: list = [0.05, 0.05, 0.05] + fixed_asset_init_orn_deg: float = 120.0 + fixed_asset_init_orn_range_deg: float = 30.0 + + # Held Asset (applies to all tasks) + held_asset_pos_noise: list = [0.0, 0.003, 0.003] # noise level of the held asset in gripper + held_asset_rot_init: float = -90.0 + + # Reward. + ee_success_yaw = 0.0 + keypoint_coef_baseline: list = [100, 2] + keypoint_coef_coarse: list = [500, 2] # 100, 2 + keypoint_coef_fine: list = [1500, 0] # 500, 0 + # Fraction of thread-height. + success_threshold: float = 0.375 + engage_threshold: float = 0.5 + keypoint_scale: float = 0.05 + + fixed_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/FixedAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=fixed_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=fixed_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.6, 0.0, 0.05), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) + held_asset: ArticulationCfg = ArticulationCfg( + prim_path="/World/envs/env_.*/HeldAsset", + spawn=sim_utils.UsdFileCfg( + usd_path=held_asset_cfg.usd_path, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=3666.0, + enable_gyroscopic_forces=True, + solver_position_iteration_count=192, + solver_velocity_iteration_count=1, + max_contact_impulse=1e32, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=held_asset_cfg.mass), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.4, 0.1), rot=(1.0, 0.0, 0.0, 0.0), joint_pos={}, joint_vel={} + ), + actuators={}, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/__init__.py new file mode 100644 index 00000000000..bea3dc84c16 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/__init__.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +""" +Franka-Cabinet environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Franka-Cabinet-Direct-v0", + entry_point=f"{__name__}.franka_cabinet_env:FrankaCabinetEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.franka_cabinet_env:FrankaCabinetEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaCabinetPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..dd9cb448ae7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,75 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: franka_cabinet_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + # value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 100000000 + max_epochs: 1500 + save_best_after: 200 + save_frequency: 100 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 8192 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..8ecf9f74799 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class FrankaCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 1500 + save_interval = 50 + experiment_name = "franka_cabinet_direct" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=8, + num_mini_batches=8, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.008, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..ca858319e60 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "franka_cabinet_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 24000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py new file mode 100644 index 00000000000..a71fa18f960 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -0,0 +1,496 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch + +from isaacsim.core.utils.stage import get_current_stage +from isaacsim.core.utils.torch.transformations import tf_combine, tf_inverse, tf_vector +from pxr import UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import sample_uniform + + +@configclass +class FrankaCabinetEnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 8.3333 # 500 timesteps + decimation = 2 + action_space = 9 + observation_space = 23 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 120, + render_interval=decimation, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=3.0, replicate_physics=True) + + # robot + robot = ArticulationCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka_instanceable.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=12, solver_velocity_iteration_count=1 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 1.157, + "panda_joint2": -1.066, + "panda_joint3": -0.155, + "panda_joint4": -2.239, + "panda_joint5": -1.841, + "panda_joint6": 1.003, + "panda_joint7": 0.469, + "panda_finger_joint.*": 0.035, + }, + pos=(1.0, 0.0, 0.0), + rot=(0.0, 0.0, 0.0, 1.0), + ), + actuators={ + "panda_shoulder": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + effort_limit=87.0, + velocity_limit=2.175, + stiffness=80.0, + damping=4.0, + ), + "panda_forearm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + effort_limit=12.0, + velocity_limit=2.61, + stiffness=80.0, + damping=4.0, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint.*"], + effort_limit=200.0, + velocity_limit=0.2, + stiffness=2e3, + damping=1e2, + ), + }, + ) + + # cabinet + cabinet = ArticulationCfg( + prim_path="/World/envs/env_.*/Cabinet", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Sektion_Cabinet/sektion_cabinet_instanceable.usd", + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0, 0.4), + rot=(0.1, 0.0, 0.0, 0.0), + joint_pos={ + "door_left_joint": 0.0, + "door_right_joint": 0.0, + "drawer_bottom_joint": 0.0, + "drawer_top_joint": 0.0, + }, + ), + actuators={ + "drawers": ImplicitActuatorCfg( + joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=10.0, + damping=1.0, + ), + "doors": ImplicitActuatorCfg( + joint_names_expr=["door_left_joint", "door_right_joint"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=10.0, + damping=2.5, + ), + }, + ) + + # ground plane + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + ) + + action_scale = 7.5 + dof_velocity_scale = 0.1 + + # reward scales + dist_reward_scale = 1.5 + rot_reward_scale = 1.5 + open_reward_scale = 10.0 + action_penalty_scale = 0.05 + finger_reward_scale = 2.0 + + +class FrankaCabinetEnv(DirectRLEnv): + # pre-physics step calls + # |-- _pre_physics_step(action) + # |-- _apply_action() + # post-physics step calls + # |-- _get_dones() + # |-- _get_rewards() + # |-- _reset_idx(env_ids) + # |-- _get_observations() + + cfg: FrankaCabinetEnvCfg + + def __init__(self, cfg: FrankaCabinetEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + def get_env_local_pose(env_pos: torch.Tensor, xformable: UsdGeom.Xformable, device: torch.device): + """Compute pose in env-local coordinates""" + world_transform = xformable.ComputeLocalToWorldTransform(0) + world_pos = world_transform.ExtractTranslation() + world_quat = world_transform.ExtractRotationQuat() + + px = world_pos[0] - env_pos[0] + py = world_pos[1] - env_pos[1] + pz = world_pos[2] - env_pos[2] + qx = world_quat.imaginary[0] + qy = world_quat.imaginary[1] + qz = world_quat.imaginary[2] + qw = world_quat.real + + return torch.tensor([px, py, pz, qw, qx, qy, qz], device=device) + + self.dt = self.cfg.sim.dt * self.cfg.decimation + + # create auxiliary variables for computing applied action, observations and rewards + self.robot_dof_lower_limits = self._robot.data.soft_joint_pos_limits[0, :, 0].to(device=self.device) + self.robot_dof_upper_limits = self._robot.data.soft_joint_pos_limits[0, :, 1].to(device=self.device) + + self.robot_dof_speed_scales = torch.ones_like(self.robot_dof_lower_limits) + self.robot_dof_speed_scales[self._robot.find_joints("panda_finger_joint1")[0]] = 0.1 + self.robot_dof_speed_scales[self._robot.find_joints("panda_finger_joint2")[0]] = 0.1 + + self.robot_dof_targets = torch.zeros((self.num_envs, self._robot.num_joints), device=self.device) + + stage = get_current_stage() + hand_pose = get_env_local_pose( + self.scene.env_origins[0], + UsdGeom.Xformable(stage.GetPrimAtPath("/World/envs/env_0/Robot/panda_link7")), + self.device, + ) + lfinger_pose = get_env_local_pose( + self.scene.env_origins[0], + UsdGeom.Xformable(stage.GetPrimAtPath("/World/envs/env_0/Robot/panda_leftfinger")), + self.device, + ) + rfinger_pose = get_env_local_pose( + self.scene.env_origins[0], + UsdGeom.Xformable(stage.GetPrimAtPath("/World/envs/env_0/Robot/panda_rightfinger")), + self.device, + ) + + finger_pose = torch.zeros(7, device=self.device) + finger_pose[0:3] = (lfinger_pose[0:3] + rfinger_pose[0:3]) / 2.0 + finger_pose[3:7] = lfinger_pose[3:7] + hand_pose_inv_rot, hand_pose_inv_pos = tf_inverse(hand_pose[3:7], hand_pose[0:3]) + + robot_local_grasp_pose_rot, robot_local_pose_pos = tf_combine( + hand_pose_inv_rot, hand_pose_inv_pos, finger_pose[3:7], finger_pose[0:3] + ) + robot_local_pose_pos += torch.tensor([0, 0.04, 0], device=self.device) + self.robot_local_grasp_pos = robot_local_pose_pos.repeat((self.num_envs, 1)) + self.robot_local_grasp_rot = robot_local_grasp_pose_rot.repeat((self.num_envs, 1)) + + drawer_local_grasp_pose = torch.tensor([0.3, 0.01, 0.0, 1.0, 0.0, 0.0, 0.0], device=self.device) + self.drawer_local_grasp_pos = drawer_local_grasp_pose[0:3].repeat((self.num_envs, 1)) + self.drawer_local_grasp_rot = drawer_local_grasp_pose[3:7].repeat((self.num_envs, 1)) + + self.gripper_forward_axis = torch.tensor([0, 0, 1], device=self.device, dtype=torch.float32).repeat( + (self.num_envs, 1) + ) + self.drawer_inward_axis = torch.tensor([-1, 0, 0], device=self.device, dtype=torch.float32).repeat( + (self.num_envs, 1) + ) + self.gripper_up_axis = torch.tensor([0, 1, 0], device=self.device, dtype=torch.float32).repeat( + (self.num_envs, 1) + ) + self.drawer_up_axis = torch.tensor([0, 0, 1], device=self.device, dtype=torch.float32).repeat( + (self.num_envs, 1) + ) + + self.hand_link_idx = self._robot.find_bodies("panda_link7")[0][0] + self.left_finger_link_idx = self._robot.find_bodies("panda_leftfinger")[0][0] + self.right_finger_link_idx = self._robot.find_bodies("panda_rightfinger")[0][0] + self.drawer_link_idx = self._cabinet.find_bodies("drawer_top")[0][0] + + self.robot_grasp_rot = torch.zeros((self.num_envs, 4), device=self.device) + self.robot_grasp_pos = torch.zeros((self.num_envs, 3), device=self.device) + self.drawer_grasp_rot = torch.zeros((self.num_envs, 4), device=self.device) + self.drawer_grasp_pos = torch.zeros((self.num_envs, 3), device=self.device) + + def _setup_scene(self): + self._robot = Articulation(self.cfg.robot) + self._cabinet = Articulation(self.cfg.cabinet) + self.scene.articulations["robot"] = self._robot + self.scene.articulations["cabinet"] = self._cabinet + + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self._terrain = self.cfg.terrain.class_type(self.cfg.terrain) + + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + # pre-physics step calls + + def _pre_physics_step(self, actions: torch.Tensor): + self.actions = actions.clone().clamp(-1.0, 1.0) + targets = self.robot_dof_targets + self.robot_dof_speed_scales * self.dt * self.actions * self.cfg.action_scale + self.robot_dof_targets[:] = torch.clamp(targets, self.robot_dof_lower_limits, self.robot_dof_upper_limits) + + def _apply_action(self): + self._robot.set_joint_position_target(self.robot_dof_targets) + + # post-physics step calls + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + terminated = self._cabinet.data.joint_pos[:, 3] > 0.39 + truncated = self.episode_length_buf >= self.max_episode_length - 1 + return terminated, truncated + + def _get_rewards(self) -> torch.Tensor: + # Refresh the intermediate values after the physics steps + self._compute_intermediate_values() + robot_left_finger_pos = self._robot.data.body_pos_w[:, self.left_finger_link_idx] + robot_right_finger_pos = self._robot.data.body_pos_w[:, self.right_finger_link_idx] + + return self._compute_rewards( + self.actions, + self._cabinet.data.joint_pos, + self.robot_grasp_pos, + self.drawer_grasp_pos, + self.robot_grasp_rot, + self.drawer_grasp_rot, + robot_left_finger_pos, + robot_right_finger_pos, + self.gripper_forward_axis, + self.drawer_inward_axis, + self.gripper_up_axis, + self.drawer_up_axis, + self.num_envs, + self.cfg.dist_reward_scale, + self.cfg.rot_reward_scale, + self.cfg.open_reward_scale, + self.cfg.action_penalty_scale, + self.cfg.finger_reward_scale, + self._robot.data.joint_pos, + ) + + def _reset_idx(self, env_ids: torch.Tensor | None): + super()._reset_idx(env_ids) + # robot state + joint_pos = self._robot.data.default_joint_pos[env_ids] + sample_uniform( + -0.125, + 0.125, + (len(env_ids), self._robot.num_joints), + self.device, + ) + joint_pos = torch.clamp(joint_pos, self.robot_dof_lower_limits, self.robot_dof_upper_limits) + joint_vel = torch.zeros_like(joint_pos) + self._robot.set_joint_position_target(joint_pos, env_ids=env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + # cabinet state + zeros = torch.zeros((len(env_ids), self._cabinet.num_joints), device=self.device) + self._cabinet.write_joint_state_to_sim(zeros, zeros, env_ids=env_ids) + + # Need to refresh the intermediate values so that _get_observations() can use the latest values + self._compute_intermediate_values(env_ids) + + def _get_observations(self) -> dict: + dof_pos_scaled = ( + 2.0 + * (self._robot.data.joint_pos - self.robot_dof_lower_limits) + / (self.robot_dof_upper_limits - self.robot_dof_lower_limits) + - 1.0 + ) + to_target = self.drawer_grasp_pos - self.robot_grasp_pos + + obs = torch.cat( + ( + dof_pos_scaled, + self._robot.data.joint_vel * self.cfg.dof_velocity_scale, + to_target, + self._cabinet.data.joint_pos[:, 3].unsqueeze(-1), + self._cabinet.data.joint_vel[:, 3].unsqueeze(-1), + ), + dim=-1, + ) + return {"policy": torch.clamp(obs, -5.0, 5.0)} + + # auxiliary methods + + def _compute_intermediate_values(self, env_ids: torch.Tensor | None = None): + if env_ids is None: + env_ids = self._robot._ALL_INDICES + + hand_pos = self._robot.data.body_pos_w[env_ids, self.hand_link_idx] + hand_rot = self._robot.data.body_quat_w[env_ids, self.hand_link_idx] + drawer_pos = self._cabinet.data.body_pos_w[env_ids, self.drawer_link_idx] + drawer_rot = self._cabinet.data.body_quat_w[env_ids, self.drawer_link_idx] + ( + self.robot_grasp_rot[env_ids], + self.robot_grasp_pos[env_ids], + self.drawer_grasp_rot[env_ids], + self.drawer_grasp_pos[env_ids], + ) = self._compute_grasp_transforms( + hand_rot, + hand_pos, + self.robot_local_grasp_rot[env_ids], + self.robot_local_grasp_pos[env_ids], + drawer_rot, + drawer_pos, + self.drawer_local_grasp_rot[env_ids], + self.drawer_local_grasp_pos[env_ids], + ) + + def _compute_rewards( + self, + actions, + cabinet_dof_pos, + franka_grasp_pos, + drawer_grasp_pos, + franka_grasp_rot, + drawer_grasp_rot, + franka_lfinger_pos, + franka_rfinger_pos, + gripper_forward_axis, + drawer_inward_axis, + gripper_up_axis, + drawer_up_axis, + num_envs, + dist_reward_scale, + rot_reward_scale, + open_reward_scale, + action_penalty_scale, + finger_reward_scale, + joint_positions, + ): + # distance from hand to the drawer + d = torch.norm(franka_grasp_pos - drawer_grasp_pos, p=2, dim=-1) + dist_reward = 1.0 / (1.0 + d**2) + dist_reward *= dist_reward + dist_reward = torch.where(d <= 0.02, dist_reward * 2, dist_reward) + + axis1 = tf_vector(franka_grasp_rot, gripper_forward_axis) + axis2 = tf_vector(drawer_grasp_rot, drawer_inward_axis) + axis3 = tf_vector(franka_grasp_rot, gripper_up_axis) + axis4 = tf_vector(drawer_grasp_rot, drawer_up_axis) + + dot1 = ( + torch.bmm(axis1.view(num_envs, 1, 3), axis2.view(num_envs, 3, 1)).squeeze(-1).squeeze(-1) + ) # alignment of forward axis for gripper + dot2 = ( + torch.bmm(axis3.view(num_envs, 1, 3), axis4.view(num_envs, 3, 1)).squeeze(-1).squeeze(-1) + ) # alignment of up axis for gripper + # reward for matching the orientation of the hand to the drawer (fingers wrapped) + rot_reward = 0.5 * (torch.sign(dot1) * dot1**2 + torch.sign(dot2) * dot2**2) + + # regularization on the actions (summed for each environment) + action_penalty = torch.sum(actions**2, dim=-1) + + # how far the cabinet has been opened out + open_reward = cabinet_dof_pos[:, 3] # drawer_top_joint + + # penalty for distance of each finger from the drawer handle + lfinger_dist = franka_lfinger_pos[:, 2] - drawer_grasp_pos[:, 2] + rfinger_dist = drawer_grasp_pos[:, 2] - franka_rfinger_pos[:, 2] + finger_dist_penalty = torch.zeros_like(lfinger_dist) + finger_dist_penalty += torch.where(lfinger_dist < 0, lfinger_dist, torch.zeros_like(lfinger_dist)) + finger_dist_penalty += torch.where(rfinger_dist < 0, rfinger_dist, torch.zeros_like(rfinger_dist)) + + rewards = ( + dist_reward_scale * dist_reward + + rot_reward_scale * rot_reward + + open_reward_scale * open_reward + + finger_reward_scale * finger_dist_penalty + - action_penalty_scale * action_penalty + ) + + self.extras["log"] = { + "dist_reward": (dist_reward_scale * dist_reward).mean(), + "rot_reward": (rot_reward_scale * rot_reward).mean(), + "open_reward": (open_reward_scale * open_reward).mean(), + "action_penalty": (-action_penalty_scale * action_penalty).mean(), + "left_finger_distance_reward": (finger_reward_scale * lfinger_dist).mean(), + "right_finger_distance_reward": (finger_reward_scale * rfinger_dist).mean(), + "finger_dist_penalty": (finger_reward_scale * finger_dist_penalty).mean(), + } + + # bonus for opening drawer properly + rewards = torch.where(cabinet_dof_pos[:, 3] > 0.01, rewards + 0.25, rewards) + rewards = torch.where(cabinet_dof_pos[:, 3] > 0.2, rewards + 0.25, rewards) + rewards = torch.where(cabinet_dof_pos[:, 3] > 0.35, rewards + 0.25, rewards) + + return rewards + + def _compute_grasp_transforms( + self, + hand_rot, + hand_pos, + franka_local_grasp_rot, + franka_local_grasp_pos, + drawer_rot, + drawer_pos, + drawer_local_grasp_rot, + drawer_local_grasp_pos, + ): + global_franka_rot, global_franka_pos = tf_combine( + hand_rot, hand_pos, franka_local_grasp_rot, franka_local_grasp_pos + ) + global_drawer_rot, global_drawer_pos = tf_combine( + drawer_rot, drawer_pos, drawer_local_grasp_rot, drawer_local_grasp_pos + ) + + return global_franka_rot, global_franka_pos, global_drawer_rot, global_drawer_pos diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/__init__.py new file mode 100644 index 00000000000..ed0a7c11e3c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Humanoid locomotion environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Humanoid-Direct-v0", + entry_point=f"{__name__}.humanoid_env:HumanoidEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.humanoid_env:HumanoidEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..0b54af26e5b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,75 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [400, 200, 100] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: humanoid_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 1000 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..3a184905961 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 32 + max_iterations = 1000 + save_interval = 50 + experiment_name = "humanoid_direct" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[400, 200, 100], + critic_hidden_dims=[400, 200, 100], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.008, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..b3ffec362d2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [400, 200, 100] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [400, 200, 100] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "humanoid_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/humanoid_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/humanoid_env.py new file mode 100644 index 00000000000..b7d06f5a07f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid/humanoid_env.py @@ -0,0 +1,100 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab_assets import HUMANOID_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv + + +@configclass +class HumanoidEnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 15.0 + decimation = 2 + action_scale = 1.0 + action_space = 21 + observation_space = 75 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="average", + restitution_combine_mode="average", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # robot + robot: ArticulationCfg = HUMANOID_CFG.replace(prim_path="/World/envs/env_.*/Robot") + joint_gears: list = [ + 67.5000, # lower_waist + 67.5000, # lower_waist + 67.5000, # right_upper_arm + 67.5000, # right_upper_arm + 67.5000, # left_upper_arm + 67.5000, # left_upper_arm + 67.5000, # pelvis + 45.0000, # right_lower_arm + 45.0000, # left_lower_arm + 45.0000, # right_thigh: x + 135.0000, # right_thigh: y + 45.0000, # right_thigh: z + 45.0000, # left_thigh: x + 135.0000, # left_thigh: y + 45.0000, # left_thigh: z + 90.0000, # right_knee + 90.0000, # left_knee + 22.5, # right_foot + 22.5, # right_foot + 22.5, # left_foot + 22.5, # left_foot + ] + + heading_weight: float = 0.5 + up_weight: float = 0.1 + + energy_cost_scale: float = 0.05 + actions_cost_scale: float = 0.01 + alive_reward_scale: float = 2.0 + dof_vel_scale: float = 0.1 + + death_cost: float = -1.0 + termination_height: float = 0.8 + + angular_velocity_scale: float = 0.25 + contact_force_scale: float = 0.01 + + +class HumanoidEnv(LocomotionEnv): + cfg: HumanoidEnvCfg + + def __init__(self, cfg: HumanoidEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/__init__.py new file mode 100644 index 00000000000..0bce6e00c53 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/__init__.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +AMP Humanoid locomotion environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Humanoid-AMP-Dance-Direct-v0", + entry_point=f"{__name__}.humanoid_amp_env:HumanoidAmpEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.humanoid_amp_env_cfg:HumanoidAmpDanceEnvCfg", + "skrl_amp_cfg_entry_point": f"{agents.__name__}:skrl_dance_amp_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Humanoid-AMP-Run-Direct-v0", + entry_point=f"{__name__}.humanoid_amp_env:HumanoidAmpEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.humanoid_amp_env_cfg:HumanoidAmpRunEnvCfg", + "skrl_amp_cfg_entry_point": f"{agents.__name__}:skrl_run_amp_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Humanoid-AMP-Walk-Direct-v0", + entry_point=f"{__name__}.humanoid_amp_env:HumanoidAmpEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.humanoid_amp_env_cfg:HumanoidAmpWalkEnvCfg", + "skrl_amp_cfg_entry_point": f"{agents.__name__}:skrl_walk_amp_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml new file mode 100644 index 00000000000..b64fe9b1cc9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml @@ -0,0 +1,111 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: -2.9 + fixed_log_std: True + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + discriminator: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + +# AMP memory (reference motion dataset) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +motion_dataset: + class: RandomMemory + memory_size: 200000 + +# AMP memory (preventing discriminator overfitting) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +reply_buffer: + class: RandomMemory + memory_size: 1000000 + + +# AMP agent configuration (field names are from AMP_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/amp.html +agent: + class: AMP + rollouts: 16 + learning_epochs: 6 + mini_batches: 2 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-05 + learning_rate_scheduler: null + learning_rate_scheduler_kwargs: null + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + amp_state_preprocessor: RunningStandardScaler + amp_state_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 0.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.5 + discriminator_loss_scale: 5.0 + amp_batch_size: 512 + task_reward_weight: 0.0 + style_reward_weight: 1.0 + discriminator_batch_size: 4096 + discriminator_reward_scale: 2.0 + discriminator_logit_regularization_scale: 0.05 + discriminator_gradient_penalty_scale: 5.0 + discriminator_weight_decay_scale: 1.0e-04 + # rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "humanoid_amp_dance" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 80000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml new file mode 100644 index 00000000000..e435b44eac9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml @@ -0,0 +1,111 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: -2.9 + fixed_log_std: True + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + discriminator: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + +# AMP memory (reference motion dataset) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +motion_dataset: + class: RandomMemory + memory_size: 200000 + +# AMP memory (preventing discriminator overfitting) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +reply_buffer: + class: RandomMemory + memory_size: 1000000 + + +# AMP agent configuration (field names are from AMP_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/amp.html +agent: + class: AMP + rollouts: 16 + learning_epochs: 6 + mini_batches: 2 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-05 + learning_rate_scheduler: null + learning_rate_scheduler_kwargs: null + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + amp_state_preprocessor: RunningStandardScaler + amp_state_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 0.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.5 + discriminator_loss_scale: 5.0 + amp_batch_size: 512 + task_reward_weight: 0.0 + style_reward_weight: 1.0 + discriminator_batch_size: 4096 + discriminator_reward_scale: 2.0 + discriminator_logit_regularization_scale: 0.05 + discriminator_gradient_penalty_scale: 5.0 + discriminator_weight_decay_scale: 1.0e-04 + # rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "humanoid_amp_run" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 80000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml new file mode 100644 index 00000000000..f3784200876 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml @@ -0,0 +1,111 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: -2.9 + fixed_log_std: True + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + discriminator: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [1024, 512] + activations: relu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + +# AMP memory (reference motion dataset) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +motion_dataset: + class: RandomMemory + memory_size: 200000 + +# AMP memory (preventing discriminator overfitting) +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +reply_buffer: + class: RandomMemory + memory_size: 1000000 + + +# AMP agent configuration (field names are from AMP_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/amp.html +agent: + class: AMP + rollouts: 16 + learning_epochs: 6 + mini_batches: 2 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-05 + learning_rate_scheduler: null + learning_rate_scheduler_kwargs: null + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + amp_state_preprocessor: RunningStandardScaler + amp_state_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 0.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.5 + discriminator_loss_scale: 5.0 + amp_batch_size: 512 + task_reward_weight: 0.0 + style_reward_weight: 1.0 + discriminator_batch_size: 4096 + discriminator_reward_scale: 2.0 + discriminator_logit_regularization_scale: 0.05 + discriminator_gradient_penalty_scale: 5.0 + discriminator_weight_decay_scale: 1.0e-04 + # rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "humanoid_amp_walk" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 80000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py new file mode 100644 index 00000000000..8ab9139e917 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env.py @@ -0,0 +1,243 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import numpy as np +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.math import quat_apply + +from .humanoid_amp_env_cfg import HumanoidAmpEnvCfg +from .motions import MotionLoader + + +class HumanoidAmpEnv(DirectRLEnv): + cfg: HumanoidAmpEnvCfg + + def __init__(self, cfg: HumanoidAmpEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + # action offset and scale + dof_lower_limits = self.robot.data.soft_joint_pos_limits[0, :, 0] + dof_upper_limits = self.robot.data.soft_joint_pos_limits[0, :, 1] + self.action_offset = 0.5 * (dof_upper_limits + dof_lower_limits) + self.action_scale = dof_upper_limits - dof_lower_limits + + # load motion + self._motion_loader = MotionLoader(motion_file=self.cfg.motion_file, device=self.device) + + # DOF and key body indexes + key_body_names = ["right_hand", "left_hand", "right_foot", "left_foot"] + self.ref_body_index = self.robot.data.body_names.index(self.cfg.reference_body) + self.key_body_indexes = [self.robot.data.body_names.index(name) for name in key_body_names] + self.motion_dof_indexes = self._motion_loader.get_dof_index(self.robot.data.joint_names) + self.motion_ref_body_index = self._motion_loader.get_body_index([self.cfg.reference_body])[0] + self.motion_key_body_indexes = self._motion_loader.get_body_index(key_body_names) + + # reconfigure AMP observation space according to the number of observations and create the buffer + self.amp_observation_size = self.cfg.num_amp_observations * self.cfg.amp_observation_space + self.amp_observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(self.amp_observation_size,)) + self.amp_observation_buffer = torch.zeros( + (self.num_envs, self.cfg.num_amp_observations, self.cfg.amp_observation_space), device=self.device + ) + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot) + # add ground plane + spawn_ground_plane( + prim_path="/World/ground", + cfg=GroundPlaneCfg( + physics_material=sim_utils.RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + ), + ) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene + self.scene.articulations["robot"] = self.robot + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor): + self.actions = actions.clone() + + def _apply_action(self): + target = self.action_offset + self.action_scale * self.actions + self.robot.set_joint_position_target(target) + + def _get_observations(self) -> dict: + # build task observation + obs = compute_obs( + self.robot.data.joint_pos, + self.robot.data.joint_vel, + self.robot.data.body_pos_w[:, self.ref_body_index], + self.robot.data.body_quat_w[:, self.ref_body_index], + self.robot.data.body_lin_vel_w[:, self.ref_body_index], + self.robot.data.body_ang_vel_w[:, self.ref_body_index], + self.robot.data.body_pos_w[:, self.key_body_indexes], + ) + + # update AMP observation history + for i in reversed(range(self.cfg.num_amp_observations - 1)): + self.amp_observation_buffer[:, i + 1] = self.amp_observation_buffer[:, i] + # build AMP observation + self.amp_observation_buffer[:, 0] = obs.clone() + self.extras = {"amp_obs": self.amp_observation_buffer.view(-1, self.amp_observation_size)} + + return {"policy": obs} + + def _get_rewards(self) -> torch.Tensor: + return torch.ones((self.num_envs,), dtype=torch.float32, device=self.sim.device) + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + time_out = self.episode_length_buf >= self.max_episode_length - 1 + if self.cfg.early_termination: + died = self.robot.data.body_pos_w[:, self.ref_body_index, 2] < self.cfg.termination_height + else: + died = torch.zeros_like(time_out) + return died, time_out + + def _reset_idx(self, env_ids: torch.Tensor | None): + if env_ids is None or len(env_ids) == self.num_envs: + env_ids = self.robot._ALL_INDICES + self.robot.reset(env_ids) + super()._reset_idx(env_ids) + + if self.cfg.reset_strategy == "default": + root_state, joint_pos, joint_vel = self._reset_strategy_default(env_ids) + elif self.cfg.reset_strategy.startswith("random"): + start = "start" in self.cfg.reset_strategy + root_state, joint_pos, joint_vel = self._reset_strategy_random(env_ids, start) + else: + raise ValueError(f"Unknown reset strategy: {self.cfg.reset_strategy}") + + self.robot.write_root_link_pose_to_sim(root_state[:, :7], env_ids) + self.robot.write_root_com_velocity_to_sim(root_state[:, 7:], env_ids) + self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + # reset strategies + + def _reset_strategy_default(self, env_ids: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + root_state = self.robot.data.default_root_state[env_ids].clone() + root_state[:, :3] += self.scene.env_origins[env_ids] + joint_pos = self.robot.data.default_joint_pos[env_ids].clone() + joint_vel = self.robot.data.default_joint_vel[env_ids].clone() + return root_state, joint_pos, joint_vel + + def _reset_strategy_random( + self, env_ids: torch.Tensor, start: bool = False + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + # sample random motion times (or zeros if start is True) + num_samples = env_ids.shape[0] + times = np.zeros(num_samples) if start else self._motion_loader.sample_times(num_samples) + # sample random motions + ( + dof_positions, + dof_velocities, + body_positions, + body_rotations, + body_linear_velocities, + body_angular_velocities, + ) = self._motion_loader.sample(num_samples=num_samples, times=times) + + # get root transforms (the humanoid torso) + motion_torso_index = self._motion_loader.get_body_index(["torso"])[0] + root_state = self.robot.data.default_root_state[env_ids].clone() + root_state[:, 0:3] = body_positions[:, motion_torso_index] + self.scene.env_origins[env_ids] + root_state[:, 2] += 0.15 # lift the humanoid slightly to avoid collisions with the ground + root_state[:, 3:7] = body_rotations[:, motion_torso_index] + root_state[:, 7:10] = body_linear_velocities[:, motion_torso_index] + root_state[:, 10:13] = body_angular_velocities[:, motion_torso_index] + # get DOFs state + dof_pos = dof_positions[:, self.motion_dof_indexes] + dof_vel = dof_velocities[:, self.motion_dof_indexes] + + # update AMP observation + amp_observations = self.collect_reference_motions(num_samples, times) + self.amp_observation_buffer[env_ids] = amp_observations.view(num_samples, self.cfg.num_amp_observations, -1) + + return root_state, dof_pos, dof_vel + + # env methods + + def collect_reference_motions(self, num_samples: int, current_times: np.ndarray | None = None) -> torch.Tensor: + # sample random motion times (or use the one specified) + if current_times is None: + current_times = self._motion_loader.sample_times(num_samples) + times = ( + np.expand_dims(current_times, axis=-1) + - self._motion_loader.dt * np.arange(0, self.cfg.num_amp_observations) + ).flatten() + # get motions + ( + dof_positions, + dof_velocities, + body_positions, + body_rotations, + body_linear_velocities, + body_angular_velocities, + ) = self._motion_loader.sample(num_samples=num_samples, times=times) + # compute AMP observation + amp_observation = compute_obs( + dof_positions[:, self.motion_dof_indexes], + dof_velocities[:, self.motion_dof_indexes], + body_positions[:, self.motion_ref_body_index], + body_rotations[:, self.motion_ref_body_index], + body_linear_velocities[:, self.motion_ref_body_index], + body_angular_velocities[:, self.motion_ref_body_index], + body_positions[:, self.motion_key_body_indexes], + ) + return amp_observation.view(-1, self.amp_observation_size) + + +@torch.jit.script +def quaternion_to_tangent_and_normal(q: torch.Tensor) -> torch.Tensor: + ref_tangent = torch.zeros_like(q[..., :3]) + ref_normal = torch.zeros_like(q[..., :3]) + ref_tangent[..., 0] = 1 + ref_normal[..., -1] = 1 + tangent = quat_apply(q, ref_tangent) + normal = quat_apply(q, ref_normal) + return torch.cat([tangent, normal], dim=len(tangent.shape) - 1) + + +@torch.jit.script +def compute_obs( + dof_positions: torch.Tensor, + dof_velocities: torch.Tensor, + root_positions: torch.Tensor, + root_rotations: torch.Tensor, + root_linear_velocities: torch.Tensor, + root_angular_velocities: torch.Tensor, + key_body_positions: torch.Tensor, +) -> torch.Tensor: + obs = torch.cat( + ( + dof_positions, + dof_velocities, + root_positions[:, 2:3], # root body height + quaternion_to_tangent_and_normal(root_rotations), + root_linear_velocities, + root_angular_velocities, + (key_body_positions - root_positions.unsqueeze(-2)).view(key_body_positions.shape[0], -1), + ), + dim=-1, + ) + return obs diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py new file mode 100644 index 00000000000..373e4f2f765 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/humanoid_amp_env_cfg.py @@ -0,0 +1,94 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import os +from dataclasses import MISSING + +from isaaclab_assets import HUMANOID_28_CFG + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.utils import configclass + +MOTIONS_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "motions") + + +@configclass +class HumanoidAmpEnvCfg(DirectRLEnvCfg): + """Humanoid AMP environment config (base class).""" + + # env + episode_length_s = 10.0 + decimation = 2 + + # spaces + observation_space = 81 + action_space = 28 + state_space = 0 + num_amp_observations = 2 + amp_observation_space = 81 + + early_termination = True + termination_height = 0.5 + + motion_file: str = MISSING + reference_body = "torso" + reset_strategy = "random" # default, random, random-start + """Strategy to be followed when resetting each environment (humanoid's pose and joint states). + + * default: pose and joint states are set to the initial state of the asset. + * random: pose and joint states are set by sampling motions at random, uniform times. + * random-start: pose and joint states are set by sampling motion at the start (time zero). + """ + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + render_interval=decimation, + physx=PhysxCfg( + gpu_found_lost_pairs_capacity=2**23, + gpu_total_aggregate_pairs_capacity=2**23, + ), + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=10.0, replicate_physics=True) + + # robot + robot: ArticulationCfg = HUMANOID_28_CFG.replace(prim_path="/World/envs/env_.*/Robot").replace( + actuators={ + "body": ImplicitActuatorCfg( + joint_names_expr=[".*"], + velocity_limit=100.0, + stiffness=None, + damping=None, + ), + }, + ) + + +@configclass +class HumanoidAmpDanceEnvCfg(HumanoidAmpEnvCfg): + motion_file = os.path.join(MOTIONS_DIR, "humanoid_dance.npz") + + +@configclass +class HumanoidAmpRunEnvCfg(HumanoidAmpEnvCfg): + motion_file = os.path.join(MOTIONS_DIR, "humanoid_run.npz") + + +@configclass +class HumanoidAmpWalkEnvCfg(HumanoidAmpEnvCfg): + motion_file = os.path.join(MOTIONS_DIR, "humanoid_walk.npz") diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/README.md b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/README.md new file mode 100644 index 00000000000..32b98bcaa61 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/README.md @@ -0,0 +1,33 @@ +# Motion files + +The motion files are in NumPy-file format that contains data from the skeleton DOFs and bodies that perform the motion. + +The data (accessed by key) is described in the following table, where: + +* `N` is the number of motion frames recorded +* `D` is the number of skeleton DOFs +* `B` is the number of skeleton bodies + +| Key | Dtype | Shape | Description | +| --- | ---- | ----- | ----------- | +| `fps` | int64 | () | FPS at which motion was sampled | +| `dof_names` | unicode string | (D,) | Skeleton DOF names | +| `body_names` | unicode string | (B,) | Skeleton body names | +| `dof_positions` | float32 | (N, D) | Skeleton DOF positions | +| `dof_velocities` | float32 | (N, D) | Skeleton DOF velocities | +| `body_positions` | float32 | (N, B, 3) | Skeleton body positions | +| `body_rotations` | float32 | (N, B, 4) | Skeleton body rotations (as `wxyz` quaternion) | +| `body_linear_velocities` | float32 | (N, B, 3) | Skeleton body linear velocities | +| `body_angular_velocities` | float32 | (N, B, 3) | Skeleton body angular velocities | + +## Motion visualization + +The `motion_viewer.py` file allows to visualize the skeleton motion recorded in a motion file. + +Open an terminal in the `motions` folder and run the following command. + +```bash +python motion_viewer.py --file MOTION_FILE_NAME.npz +``` + +See `python motion_viewer.py --help` for available arguments. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py new file mode 100644 index 00000000000..c63606832a8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +AMP Motion Loader and motion files. +""" + +from .motion_loader import MotionLoader +from .motion_viewer import MotionViewer diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_dance.npz b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_dance.npz new file mode 100644 index 00000000000..29064c2f1e3 Binary files /dev/null and b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_dance.npz differ diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_run.npz b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_run.npz new file mode 100644 index 00000000000..f9eee7d55a5 Binary files /dev/null and b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_run.npz differ diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_walk.npz b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_walk.npz new file mode 100644 index 00000000000..23fe2bb8863 Binary files /dev/null and b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/humanoid_walk.npz differ diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py new file mode 100644 index 00000000000..1f86aee9abd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_loader.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import os +import torch +from typing import Optional + + +class MotionLoader: + """ + Helper class to load and sample motion data from NumPy-file format. + """ + + def __init__(self, motion_file: str, device: torch.device) -> None: + """Load a motion file and initialize the internal variables. + + Args: + motion_file: Motion file path to load. + device: The device to which to load the data. + + Raises: + AssertionError: If the specified motion file doesn't exist. + """ + assert os.path.isfile(motion_file), f"Invalid file path: {motion_file}" + data = np.load(motion_file) + + self.device = device + self._dof_names = data["dof_names"].tolist() + self._body_names = data["body_names"].tolist() + + self.dof_positions = torch.tensor(data["dof_positions"], dtype=torch.float32, device=self.device) + self.dof_velocities = torch.tensor(data["dof_velocities"], dtype=torch.float32, device=self.device) + self.body_positions = torch.tensor(data["body_positions"], dtype=torch.float32, device=self.device) + self.body_rotations = torch.tensor(data["body_rotations"], dtype=torch.float32, device=self.device) + self.body_linear_velocities = torch.tensor( + data["body_linear_velocities"], dtype=torch.float32, device=self.device + ) + self.body_angular_velocities = torch.tensor( + data["body_angular_velocities"], dtype=torch.float32, device=self.device + ) + + self.dt = 1.0 / data["fps"] + self.num_frames = self.dof_positions.shape[0] + self.duration = self.dt * (self.num_frames - 1) + print(f"Motion loaded ({motion_file}): duration: {self.duration} sec, frames: {self.num_frames}") + + @property + def dof_names(self) -> list[str]: + """Skeleton DOF names.""" + return self._dof_names + + @property + def body_names(self) -> list[str]: + """Skeleton rigid body names.""" + return self._body_names + + @property + def num_dofs(self) -> int: + """Number of skeleton's DOFs.""" + return len(self._dof_names) + + @property + def num_bodies(self) -> int: + """Number of skeleton's rigid bodies.""" + return len(self._body_names) + + def _interpolate( + self, + a: torch.Tensor, + *, + b: Optional[torch.Tensor] = None, + blend: Optional[torch.Tensor] = None, + start: Optional[np.ndarray] = None, + end: Optional[np.ndarray] = None, + ) -> torch.Tensor: + """Linear interpolation between consecutive values. + + Args: + a: The first value. Shape is (N, X) or (N, M, X). + b: The second value. Shape is (N, X) or (N, M, X). + blend: Interpolation coefficient between 0 (a) and 1 (b). + start: Indexes to fetch the first value. If both, ``start`` and ``end` are specified, + the first and second values will be fetches from the argument ``a`` (dimension 0). + end: Indexes to fetch the second value. If both, ``start`` and ``end` are specified, + the first and second values will be fetches from the argument ``a`` (dimension 0). + + Returns: + Interpolated values. Shape is (N, X) or (N, M, X). + """ + if start is not None and end is not None: + return self._interpolate(a=a[start], b=a[end], blend=blend) + if a.ndim >= 2: + blend = blend.unsqueeze(-1) + if a.ndim >= 3: + blend = blend.unsqueeze(-1) + return (1.0 - blend) * a + blend * b + + def _slerp( + self, + q0: torch.Tensor, + *, + q1: Optional[torch.Tensor] = None, + blend: Optional[torch.Tensor] = None, + start: Optional[np.ndarray] = None, + end: Optional[np.ndarray] = None, + ) -> torch.Tensor: + """Interpolation between consecutive rotations (Spherical Linear Interpolation). + + Args: + q0: The first quaternion (wxyz). Shape is (N, 4) or (N, M, 4). + q1: The second quaternion (wxyz). Shape is (N, 4) or (N, M, 4). + blend: Interpolation coefficient between 0 (q0) and 1 (q1). + start: Indexes to fetch the first quaternion. If both, ``start`` and ``end` are specified, + the first and second quaternions will be fetches from the argument ``q0`` (dimension 0). + end: Indexes to fetch the second quaternion. If both, ``start`` and ``end` are specified, + the first and second quaternions will be fetches from the argument ``q0`` (dimension 0). + + Returns: + Interpolated quaternions. Shape is (N, 4) or (N, M, 4). + """ + if start is not None and end is not None: + return self._slerp(q0=q0[start], q1=q0[end], blend=blend) + if q0.ndim >= 2: + blend = blend.unsqueeze(-1) + if q0.ndim >= 3: + blend = blend.unsqueeze(-1) + + qw, qx, qy, qz = 0, 1, 2, 3 # wxyz + cos_half_theta = ( + q0[..., qw] * q1[..., qw] + + q0[..., qx] * q1[..., qx] + + q0[..., qy] * q1[..., qy] + + q0[..., qz] * q1[..., qz] + ) + + neg_mask = cos_half_theta < 0 + q1 = q1.clone() + q1[neg_mask] = -q1[neg_mask] + cos_half_theta = torch.abs(cos_half_theta) + cos_half_theta = torch.unsqueeze(cos_half_theta, dim=-1) + + half_theta = torch.acos(cos_half_theta) + sin_half_theta = torch.sqrt(1.0 - cos_half_theta * cos_half_theta) + + ratio_a = torch.sin((1 - blend) * half_theta) / sin_half_theta + ratio_b = torch.sin(blend * half_theta) / sin_half_theta + + new_q_x = ratio_a * q0[..., qx : qx + 1] + ratio_b * q1[..., qx : qx + 1] + new_q_y = ratio_a * q0[..., qy : qy + 1] + ratio_b * q1[..., qy : qy + 1] + new_q_z = ratio_a * q0[..., qz : qz + 1] + ratio_b * q1[..., qz : qz + 1] + new_q_w = ratio_a * q0[..., qw : qw + 1] + ratio_b * q1[..., qw : qw + 1] + + new_q = torch.cat([new_q_w, new_q_x, new_q_y, new_q_z], dim=len(new_q_w.shape) - 1) + new_q = torch.where(torch.abs(sin_half_theta) < 0.001, 0.5 * q0 + 0.5 * q1, new_q) + new_q = torch.where(torch.abs(cos_half_theta) >= 1, q0, new_q) + return new_q + + def _compute_frame_blend(self, times: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Compute the indexes of the first and second values, as well as the blending time + to interpolate between them and the given times. + + Args: + times: Times, between 0 and motion duration, to sample motion values. + Specified times will be clipped to fall within the range of the motion duration. + + Returns: + First value indexes, Second value indexes, and blending time between 0 (first value) and 1 (second value). + """ + phase = np.clip(times / self.duration, 0.0, 1.0) + index_0 = (phase * (self.num_frames - 1)).round(decimals=0).astype(int) + index_1 = np.minimum(index_0 + 1, self.num_frames - 1) + blend = ((times - index_0 * self.dt) / self.dt).round(decimals=5) + return index_0, index_1, blend + + def sample_times(self, num_samples: int, duration: float | None = None) -> np.ndarray: + """Sample random motion times uniformly. + + Args: + num_samples: Number of time samples to generate. + duration: Maximum motion duration to sample. + If not defined samples will be within the range of the motion duration. + + Raises: + AssertionError: If the specified duration is longer than the motion duration. + + Returns: + Time samples, between 0 and the specified/motion duration. + """ + duration = self.duration if duration is None else duration + assert ( + duration <= self.duration + ), f"The specified duration ({duration}) is longer than the motion duration ({self.duration})" + return duration * np.random.uniform(low=0.0, high=1.0, size=num_samples) + + def sample( + self, num_samples: int, times: Optional[np.ndarray] = None, duration: float | None = None + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Sample motion data. + + Args: + num_samples: Number of time samples to generate. If ``times`` is defined, this parameter is ignored. + times: Motion time used for sampling. + If not defined, motion data will be random sampled uniformly in time. + duration: Maximum motion duration to sample. + If not defined, samples will be within the range of the motion duration. + If ``times`` is defined, this parameter is ignored. + + Returns: + Sampled motion DOF positions (with shape (N, num_dofs)), DOF velocities (with shape (N, num_dofs)), + body positions (with shape (N, num_bodies, 3)), body rotations (with shape (N, num_bodies, 4), as wxyz quaternion), + body linear velocities (with shape (N, num_bodies, 3)) and body angular velocities (with shape (N, num_bodies, 3)). + """ + times = self.sample_times(num_samples, duration) if times is None else times + index_0, index_1, blend = self._compute_frame_blend(times) + blend = torch.tensor(blend, dtype=torch.float32, device=self.device) + + return ( + self._interpolate(self.dof_positions, blend=blend, start=index_0, end=index_1), + self._interpolate(self.dof_velocities, blend=blend, start=index_0, end=index_1), + self._interpolate(self.body_positions, blend=blend, start=index_0, end=index_1), + self._slerp(self.body_rotations, blend=blend, start=index_0, end=index_1), + self._interpolate(self.body_linear_velocities, blend=blend, start=index_0, end=index_1), + self._interpolate(self.body_angular_velocities, blend=blend, start=index_0, end=index_1), + ) + + def get_dof_index(self, dof_names: list[str]) -> list[int]: + """Get skeleton DOFs indexes by DOFs names. + + Args: + dof_names: List of DOFs names. + + Raises: + AssertionError: If the specified DOFs name doesn't exist. + + Returns: + List of DOFs indexes. + """ + indexes = [] + for name in dof_names: + assert name in self._dof_names, f"The specified DOF name ({name}) doesn't exist: {self._dof_names}" + indexes.append(self._dof_names.index(name)) + return indexes + + def get_body_index(self, body_names: list[str]) -> list[int]: + """Get skeleton body indexes by body names. + + Args: + dof_names: List of body names. + + Raises: + AssertionError: If the specified body name doesn't exist. + + Returns: + List of body indexes. + """ + indexes = [] + for name in body_names: + assert name in self._body_names, f"The specified body name ({name}) doesn't exist: {self._body_names}" + indexes.append(self._body_names.index(name)) + return indexes + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--file", type=str, required=True, help="Motion file") + args, _ = parser.parse_known_args() + + motion = MotionLoader(args.file, "cpu") + + print("- number of frames:", motion.num_frames) + print("- number of DOFs:", motion.num_dofs) + print("- number of bodies:", motion.num_bodies) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py new file mode 100644 index 00000000000..5b489f9211f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/humanoid_amp/motions/motion_viewer.py @@ -0,0 +1,136 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import matplotlib +import matplotlib.animation +import matplotlib.pyplot as plt +import numpy as np +import torch + +import mpl_toolkits.mplot3d # noqa: F401 + +try: + from .motion_loader import MotionLoader +except ImportError: + from motion_loader import MotionLoader + + +class MotionViewer: + """ + Helper class to visualize motion data from NumPy-file format. + """ + + def __init__(self, motion_file: str, device: torch.device | str = "cpu", render_scene: bool = False) -> None: + """Load a motion file and initialize the internal variables. + + Args: + motion_file: Motion file path to load. + device: The device to which to load the data. + render_scene: Whether the scene (space occupied by the skeleton during movement) + is rendered instead of a reduced view of the skeleton. + + Raises: + AssertionError: If the specified motion file doesn't exist. + """ + self._figure = None + self._figure_axes = None + self._render_scene = render_scene + + # load motions + self._motion_loader = MotionLoader(motion_file=motion_file, device=device) + + self._num_frames = self._motion_loader.num_frames + self._current_frame = 0 + self._body_positions = self._motion_loader.body_positions.cpu().numpy() + + print("\nBody") + for i, name in enumerate(self._motion_loader.body_names): + minimum = np.min(self._body_positions[:, i], axis=0).round(decimals=2) + maximum = np.max(self._body_positions[:, i], axis=0).round(decimals=2) + print(f" |-- [{name}] minimum position: {minimum}, maximum position: {maximum}") + + def _drawing_callback(self, frame: int) -> None: + """Drawing callback called each frame""" + # get current motion frame + # get data + vertices = self._body_positions[self._current_frame] + # draw skeleton state + self._figure_axes.clear() + self._figure_axes.scatter(*vertices.T, color="black", depthshade=False) + # adjust exes according to motion view + # - scene + if self._render_scene: + # compute axes limits + minimum = np.min(self._body_positions.reshape(-1, 3), axis=0) + maximum = np.max(self._body_positions.reshape(-1, 3), axis=0) + center = 0.5 * (maximum + minimum) + diff = 0.75 * (maximum - minimum) + # - skeleton + else: + # compute axes limits + minimum = np.min(vertices, axis=0) + maximum = np.max(vertices, axis=0) + center = 0.5 * (maximum + minimum) + diff = np.array([0.75 * np.max(maximum - minimum).item()] * 3) + # scale view + self._figure_axes.set_xlim((center[0] - diff[0], center[0] + diff[0])) + self._figure_axes.set_ylim((center[1] - diff[1], center[1] + diff[1])) + self._figure_axes.set_zlim((center[2] - diff[2], center[2] + diff[2])) + self._figure_axes.set_box_aspect(aspect=diff / diff[0]) + # plot ground plane + x, y = np.meshgrid([center[0] - diff[0], center[0] + diff[0]], [center[1] - diff[1], center[1] + diff[1]]) + self._figure_axes.plot_surface(x, y, np.zeros_like(x), color="green", alpha=0.2) + # print metadata + self._figure_axes.set_xlabel("X") + self._figure_axes.set_ylabel("Y") + self._figure_axes.set_zlabel("Z") + self._figure_axes.set_title(f"frame: {self._current_frame}/{self._num_frames}") + # increase frame counter + self._current_frame += 1 + if self._current_frame >= self._num_frames: + self._current_frame = 0 + + def show(self) -> None: + """Show motion""" + # create a 3D figure + self._figure = plt.figure() + self._figure_axes = self._figure.add_subplot(projection="3d") + # matplotlib animation (the instance must live as long as the animation will run) + self._animation = matplotlib.animation.FuncAnimation( + fig=self._figure, + func=self._drawing_callback, + frames=self._num_frames, + interval=1000 * self._motion_loader.dt, + ) + plt.show() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--file", type=str, required=True, help="Motion file") + parser.add_argument( + "--render-scene", + action="store_true", + default=False, + help=( + "Whether the scene (space occupied by the skeleton during movement) is rendered instead of a reduced view" + " of the skeleton." + ), + ) + parser.add_argument("--matplotlib-backend", type=str, default="TkAgg", help="Matplotlib interactive backend") + args, _ = parser.parse_known_args() + + # https://matplotlib.org/stable/users/explain/figure/backends.html#interactive-backends + matplotlib.use(args.matplotlib_backend) + + viewer = MotionViewer(args.file, render_scene=args.render_scene) + viewer.show() diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py new file mode 100644 index 00000000000..6a497e551ed --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py @@ -0,0 +1,438 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import numpy as np +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs import DirectRLEnv +from isaaclab.markers import VisualizationMarkers +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.math import quat_conjugate, quat_from_angle_axis, quat_mul, sample_uniform, saturate + +if TYPE_CHECKING: + from isaaclab_tasks.direct.allegro_hand.allegro_hand_env_cfg import AllegroHandEnvCfg + from isaaclab_tasks.direct.shadow_hand.shadow_hand_env_cfg import ShadowHandEnvCfg + + +class InHandManipulationEnv(DirectRLEnv): + cfg: AllegroHandEnvCfg | ShadowHandEnvCfg + + def __init__(self, cfg: AllegroHandEnvCfg | ShadowHandEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self.num_hand_dofs = self.hand.num_joints + + # buffers for position targets + self.hand_dof_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) + self.prev_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) + self.cur_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) + + # list of actuated joints + self.actuated_dof_indices = list() + for joint_name in cfg.actuated_joint_names: + self.actuated_dof_indices.append(self.hand.joint_names.index(joint_name)) + self.actuated_dof_indices.sort() + + # finger bodies + self.finger_bodies = list() + for body_name in self.cfg.fingertip_body_names: + self.finger_bodies.append(self.hand.body_names.index(body_name)) + self.finger_bodies.sort() + self.num_fingertips = len(self.finger_bodies) + + # joint limits + joint_pos_limits = self.hand.root_physx_view.get_dof_limits().to(self.device) + self.hand_dof_lower_limits = joint_pos_limits[..., 0] + self.hand_dof_upper_limits = joint_pos_limits[..., 1] + + # track goal resets + self.reset_goal_buf = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + # used to compare object position + self.in_hand_pos = self.object.data.default_root_state[:, 0:3].clone() + self.in_hand_pos[:, 2] -= 0.04 + # default goal positions + self.goal_rot = torch.zeros((self.num_envs, 4), dtype=torch.float, device=self.device) + self.goal_rot[:, 0] = 1.0 + self.goal_pos = torch.zeros((self.num_envs, 3), dtype=torch.float, device=self.device) + self.goal_pos[:, :] = torch.tensor([-0.2, -0.45, 0.68], device=self.device) + # initialize goal marker + self.goal_markers = VisualizationMarkers(self.cfg.goal_object_cfg) + + # track successes + self.successes = torch.zeros(self.num_envs, dtype=torch.float, device=self.device) + self.consecutive_successes = torch.zeros(1, dtype=torch.float, device=self.device) + + # unit tensors + self.x_unit_tensor = torch.tensor([1, 0, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + self.y_unit_tensor = torch.tensor([0, 1, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + self.z_unit_tensor = torch.tensor([0, 0, 1], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + + def _setup_scene(self): + # add hand, in-hand object, and goal object + self.hand = Articulation(self.cfg.robot_cfg) + self.object = RigidObject(self.cfg.object_cfg) + # add ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + # clone and replicate (no need to filter for this environment) + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene - we must register to scene to randomize with EventManager + self.scene.articulations["robot"] = self.hand + self.scene.rigid_objects["object"] = self.object + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + self.cur_targets[:, self.actuated_dof_indices] = scale( + self.actions, + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + self.cur_targets[:, self.actuated_dof_indices] = ( + self.cfg.act_moving_average * self.cur_targets[:, self.actuated_dof_indices] + + (1.0 - self.cfg.act_moving_average) * self.prev_targets[:, self.actuated_dof_indices] + ) + self.cur_targets[:, self.actuated_dof_indices] = saturate( + self.cur_targets[:, self.actuated_dof_indices], + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + + self.prev_targets[:, self.actuated_dof_indices] = self.cur_targets[:, self.actuated_dof_indices] + + self.hand.set_joint_position_target( + self.cur_targets[:, self.actuated_dof_indices], joint_ids=self.actuated_dof_indices + ) + + def _get_observations(self) -> dict: + if self.cfg.asymmetric_obs: + self.fingertip_force_sensors = self.hand.root_physx_view.get_link_incoming_joint_force()[ + :, self.finger_bodies + ] + + if self.cfg.obs_type == "openai": + obs = self.compute_reduced_observations() + elif self.cfg.obs_type == "full": + obs = self.compute_full_observations() + else: + print("Unknown observations type!") + + if self.cfg.asymmetric_obs: + states = self.compute_full_state() + + observations = {"policy": obs} + if self.cfg.asymmetric_obs: + observations = {"policy": obs, "critic": states} + return observations + + def _get_rewards(self) -> torch.Tensor: + ( + total_reward, + self.reset_goal_buf, + self.successes[:], + self.consecutive_successes[:], + ) = compute_rewards( + self.reset_buf, + self.reset_goal_buf, + self.successes, + self.consecutive_successes, + self.max_episode_length, + self.object_pos, + self.object_rot, + self.in_hand_pos, + self.goal_rot, + self.cfg.dist_reward_scale, + self.cfg.rot_reward_scale, + self.cfg.rot_eps, + self.actions, + self.cfg.action_penalty_scale, + self.cfg.success_tolerance, + self.cfg.reach_goal_bonus, + self.cfg.fall_dist, + self.cfg.fall_penalty, + self.cfg.av_factor, + ) + + if "log" not in self.extras: + self.extras["log"] = dict() + self.extras["log"]["consecutive_successes"] = self.consecutive_successes.mean() + + # reset goals if the goal has been reached + goal_env_ids = self.reset_goal_buf.nonzero(as_tuple=False).squeeze(-1) + if len(goal_env_ids) > 0: + self._reset_target_pose(goal_env_ids) + + return total_reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + self._compute_intermediate_values() + + # reset when cube has fallen + goal_dist = torch.norm(self.object_pos - self.in_hand_pos, p=2, dim=-1) + out_of_reach = goal_dist >= self.cfg.fall_dist + + if self.cfg.max_consecutive_success > 0: + # Reset progress (episode length buf) on goal envs if max_consecutive_success > 0 + rot_dist = rotation_distance(self.object_rot, self.goal_rot) + self.episode_length_buf = torch.where( + torch.abs(rot_dist) <= self.cfg.success_tolerance, + torch.zeros_like(self.episode_length_buf), + self.episode_length_buf, + ) + max_success_reached = self.successes >= self.cfg.max_consecutive_success + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + if self.cfg.max_consecutive_success > 0: + time_out = time_out | max_success_reached + return out_of_reach, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None: + env_ids = self.hand._ALL_INDICES + # resets articulation and rigid body attributes + super()._reset_idx(env_ids) + + # reset goals + self._reset_target_pose(env_ids) + + # reset object + object_default_state = self.object.data.default_root_state.clone()[env_ids] + pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), 3), device=self.device) + # global object positions + object_default_state[:, 0:3] = ( + object_default_state[:, 0:3] + self.cfg.reset_position_noise * pos_noise + self.scene.env_origins[env_ids] + ) + + rot_noise = sample_uniform(-1.0, 1.0, (len(env_ids), 2), device=self.device) # noise for X and Y rotation + object_default_state[:, 3:7] = randomize_rotation( + rot_noise[:, 0], rot_noise[:, 1], self.x_unit_tensor[env_ids], self.y_unit_tensor[env_ids] + ) + + object_default_state[:, 7:] = torch.zeros_like(self.object.data.default_root_state[env_ids, 7:]) + self.object.write_root_pose_to_sim(object_default_state[:, :7], env_ids) + self.object.write_root_velocity_to_sim(object_default_state[:, 7:], env_ids) + + # reset hand + delta_max = self.hand_dof_upper_limits[env_ids] - self.hand.data.default_joint_pos[env_ids] + delta_min = self.hand_dof_lower_limits[env_ids] - self.hand.data.default_joint_pos[env_ids] + + dof_pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + rand_delta = delta_min + (delta_max - delta_min) * 0.5 * dof_pos_noise + dof_pos = self.hand.data.default_joint_pos[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + + dof_vel_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + dof_vel = self.hand.data.default_joint_vel[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise + + self.prev_targets[env_ids] = dof_pos + self.cur_targets[env_ids] = dof_pos + self.hand_dof_targets[env_ids] = dof_pos + + self.hand.set_joint_position_target(dof_pos, env_ids=env_ids) + self.hand.write_joint_state_to_sim(dof_pos, dof_vel, env_ids=env_ids) + + self.successes[env_ids] = 0 + self._compute_intermediate_values() + + def _reset_target_pose(self, env_ids): + # reset goal rotation + rand_floats = sample_uniform(-1.0, 1.0, (len(env_ids), 2), device=self.device) + new_rot = randomize_rotation( + rand_floats[:, 0], rand_floats[:, 1], self.x_unit_tensor[env_ids], self.y_unit_tensor[env_ids] + ) + + # update goal pose and markers + self.goal_rot[env_ids] = new_rot + goal_pos = self.goal_pos + self.scene.env_origins + self.goal_markers.visualize(goal_pos, self.goal_rot) + + self.reset_goal_buf[env_ids] = 0 + + def _compute_intermediate_values(self): + # data for hand + self.fingertip_pos = self.hand.data.body_pos_w[:, self.finger_bodies] + self.fingertip_rot = self.hand.data.body_quat_w[:, self.finger_bodies] + self.fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( + self.num_envs, self.num_fingertips, 3 + ) + self.fingertip_velocities = self.hand.data.body_vel_w[:, self.finger_bodies] + + self.hand_dof_pos = self.hand.data.joint_pos + self.hand_dof_vel = self.hand.data.joint_vel + + # data for object + self.object_pos = self.object.data.root_pos_w - self.scene.env_origins + self.object_rot = self.object.data.root_quat_w + self.object_velocities = self.object.data.root_vel_w + self.object_linvel = self.object.data.root_lin_vel_w + self.object_angvel = self.object.data.root_ang_vel_w + + def compute_reduced_observations(self): + # Per https://arxiv.org/pdf/1808.00177.pdf Table 2 + # Fingertip positions + # Object Position, but not orientation + # Relative target orientation + obs = torch.cat( + ( + self.fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + self.object_pos, + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + self.actions, + ), + dim=-1, + ) + + return obs + + def compute_full_observations(self): + obs = torch.cat( + ( + # hand + unscale(self.hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + self.cfg.vel_obs_scale * self.hand_dof_vel, + # object + self.object_pos, + self.object_rot, + self.object_linvel, + self.cfg.vel_obs_scale * self.object_angvel, + # goal + self.in_hand_pos, + self.goal_rot, + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + # fingertips + self.fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + self.fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + self.fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # actions + self.actions, + ), + dim=-1, + ) + return obs + + def compute_full_state(self): + states = torch.cat( + ( + # hand + unscale(self.hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + self.cfg.vel_obs_scale * self.hand_dof_vel, + # object + self.object_pos, + self.object_rot, + self.object_linvel, + self.cfg.vel_obs_scale * self.object_angvel, + # goal + self.in_hand_pos, + self.goal_rot, + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + # fingertips + self.fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + self.fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + self.fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + self.cfg.force_torque_obs_scale + * self.fingertip_force_sensors.view(self.num_envs, self.num_fingertips * 6), + # actions + self.actions, + ), + dim=-1, + ) + return states + + +@torch.jit.script +def scale(x, lower, upper): + return 0.5 * (x + 1.0) * (upper - lower) + lower + + +@torch.jit.script +def unscale(x, lower, upper): + return (2.0 * x - upper - lower) / (upper - lower) + + +@torch.jit.script +def randomize_rotation(rand0, rand1, x_unit_tensor, y_unit_tensor): + return quat_mul( + quat_from_angle_axis(rand0 * np.pi, x_unit_tensor), quat_from_angle_axis(rand1 * np.pi, y_unit_tensor) + ) + + +@torch.jit.script +def rotation_distance(object_rot, target_rot): + # Orientation alignment for the cube in hand and goal cube + quat_diff = quat_mul(object_rot, quat_conjugate(target_rot)) + return 2.0 * torch.asin(torch.clamp(torch.norm(quat_diff[:, 1:4], p=2, dim=-1), max=1.0)) # changed quat convention + + +@torch.jit.script +def compute_rewards( + reset_buf: torch.Tensor, + reset_goal_buf: torch.Tensor, + successes: torch.Tensor, + consecutive_successes: torch.Tensor, + max_episode_length: float, + object_pos: torch.Tensor, + object_rot: torch.Tensor, + target_pos: torch.Tensor, + target_rot: torch.Tensor, + dist_reward_scale: float, + rot_reward_scale: float, + rot_eps: float, + actions: torch.Tensor, + action_penalty_scale: float, + success_tolerance: float, + reach_goal_bonus: float, + fall_dist: float, + fall_penalty: float, + av_factor: float, +): + + goal_dist = torch.norm(object_pos - target_pos, p=2, dim=-1) + rot_dist = rotation_distance(object_rot, target_rot) + + dist_rew = goal_dist * dist_reward_scale + rot_rew = 1.0 / (torch.abs(rot_dist) + rot_eps) * rot_reward_scale + + action_penalty = torch.sum(actions**2, dim=-1) + + # Total reward is: position distance + orientation alignment + action regularization + success bonus + fall penalty + reward = dist_rew + rot_rew + action_penalty * action_penalty_scale + + # Find out which envs hit the goal and update successes count + goal_resets = torch.where(torch.abs(rot_dist) <= success_tolerance, torch.ones_like(reset_goal_buf), reset_goal_buf) + successes = successes + goal_resets + + # Success bonus: orientation is within `success_tolerance` of goal orientation + reward = torch.where(goal_resets == 1, reward + reach_goal_bonus, reward) + + # Fall penalty: distance to the goal is larger than a threshold + reward = torch.where(goal_dist >= fall_dist, reward + fall_penalty, reward) + + # Check env termination conditions, including maximum success number + resets = torch.where(goal_dist >= fall_dist, torch.ones_like(reset_buf), reset_buf) + + num_resets = torch.sum(resets) + finished_cons_successes = torch.sum(successes * resets.float()) + + cons_successes = torch.where( + num_resets > 0, + av_factor * finished_cons_successes / num_resets + (1.0 - av_factor) * consecutive_successes, + consecutive_successes, + ) + + return reward, goal_resets, successes, cons_successes diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/locomotion_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/locomotion_env.py new file mode 100644 index 00000000000..24cd9ae3800 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/locomotion/locomotion_env.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch + +import isaacsim.core.utils.torch as torch_utils +from isaacsim.core.utils.torch.rotations import compute_heading_and_up, compute_rot, quat_conjugate + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg + + +def normalize_angle(x): + return torch.atan2(torch.sin(x), torch.cos(x)) + + +class LocomotionEnv(DirectRLEnv): + cfg: DirectRLEnvCfg + + def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self.action_scale = self.cfg.action_scale + self.joint_gears = torch.tensor(self.cfg.joint_gears, dtype=torch.float32, device=self.sim.device) + self.motor_effort_ratio = torch.ones_like(self.joint_gears, device=self.sim.device) + self._joint_dof_idx, _ = self.robot.find_joints(".*") + + self.potentials = torch.zeros(self.num_envs, dtype=torch.float32, device=self.sim.device) + self.prev_potentials = torch.zeros_like(self.potentials) + self.targets = torch.tensor([1000, 0, 0], dtype=torch.float32, device=self.sim.device).repeat( + (self.num_envs, 1) + ) + self.targets += self.scene.env_origins + self.start_rotation = torch.tensor([1, 0, 0, 0], device=self.sim.device, dtype=torch.float32) + self.up_vec = torch.tensor([0, 0, 1], dtype=torch.float32, device=self.sim.device).repeat((self.num_envs, 1)) + self.heading_vec = torch.tensor([1, 0, 0], dtype=torch.float32, device=self.sim.device).repeat( + (self.num_envs, 1) + ) + self.inv_start_rot = quat_conjugate(self.start_rotation).repeat((self.num_envs, 1)) + self.basis_vec0 = self.heading_vec.clone() + self.basis_vec1 = self.up_vec.clone() + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot) + # add ground plane + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self.terrain = self.cfg.terrain.class_type(self.cfg.terrain) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene + self.scene.articulations["robot"] = self.robot + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor): + self.actions = actions.clone() + + def _apply_action(self): + forces = self.action_scale * self.joint_gears * self.actions + self.robot.set_joint_effort_target(forces, joint_ids=self._joint_dof_idx) + + def _compute_intermediate_values(self): + self.torso_position, self.torso_rotation = self.robot.data.root_pos_w, self.robot.data.root_quat_w + self.velocity, self.ang_velocity = self.robot.data.root_lin_vel_w, self.robot.data.root_ang_vel_w + self.dof_pos, self.dof_vel = self.robot.data.joint_pos, self.robot.data.joint_vel + + ( + self.up_proj, + self.heading_proj, + self.up_vec, + self.heading_vec, + self.vel_loc, + self.angvel_loc, + self.roll, + self.pitch, + self.yaw, + self.angle_to_target, + self.dof_pos_scaled, + self.prev_potentials, + self.potentials, + ) = compute_intermediate_values( + self.targets, + self.torso_position, + self.torso_rotation, + self.velocity, + self.ang_velocity, + self.dof_pos, + self.robot.data.soft_joint_pos_limits[0, :, 0], + self.robot.data.soft_joint_pos_limits[0, :, 1], + self.inv_start_rot, + self.basis_vec0, + self.basis_vec1, + self.potentials, + self.prev_potentials, + self.cfg.sim.dt, + ) + + def _get_observations(self) -> dict: + obs = torch.cat( + ( + self.torso_position[:, 2].view(-1, 1), + self.vel_loc, + self.angvel_loc * self.cfg.angular_velocity_scale, + normalize_angle(self.yaw).unsqueeze(-1), + normalize_angle(self.roll).unsqueeze(-1), + normalize_angle(self.angle_to_target).unsqueeze(-1), + self.up_proj.unsqueeze(-1), + self.heading_proj.unsqueeze(-1), + self.dof_pos_scaled, + self.dof_vel * self.cfg.dof_vel_scale, + self.actions, + ), + dim=-1, + ) + observations = {"policy": obs} + return observations + + def _get_rewards(self) -> torch.Tensor: + total_reward = compute_rewards( + self.actions, + self.reset_terminated, + self.cfg.up_weight, + self.cfg.heading_weight, + self.heading_proj, + self.up_proj, + self.dof_vel, + self.dof_pos_scaled, + self.potentials, + self.prev_potentials, + self.cfg.actions_cost_scale, + self.cfg.energy_cost_scale, + self.cfg.dof_vel_scale, + self.cfg.death_cost, + self.cfg.alive_reward_scale, + self.motor_effort_ratio, + ) + return total_reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + self._compute_intermediate_values() + time_out = self.episode_length_buf >= self.max_episode_length - 1 + died = self.torso_position[:, 2] < self.cfg.termination_height + return died, time_out + + def _reset_idx(self, env_ids: torch.Tensor | None): + if env_ids is None or len(env_ids) == self.num_envs: + env_ids = self.robot._ALL_INDICES + self.robot.reset(env_ids) + super()._reset_idx(env_ids) + + joint_pos = self.robot.data.default_joint_pos[env_ids] + joint_vel = self.robot.data.default_joint_vel[env_ids] + default_root_state = self.robot.data.default_root_state[env_ids] + default_root_state[:, :3] += self.scene.env_origins[env_ids] + + self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self.robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + to_target = self.targets[env_ids] - default_root_state[:, :3] + to_target[:, 2] = 0.0 + self.potentials[env_ids] = -torch.norm(to_target, p=2, dim=-1) / self.cfg.sim.dt + + self._compute_intermediate_values() + + +@torch.jit.script +def compute_rewards( + actions: torch.Tensor, + reset_terminated: torch.Tensor, + up_weight: float, + heading_weight: float, + heading_proj: torch.Tensor, + up_proj: torch.Tensor, + dof_vel: torch.Tensor, + dof_pos_scaled: torch.Tensor, + potentials: torch.Tensor, + prev_potentials: torch.Tensor, + actions_cost_scale: float, + energy_cost_scale: float, + dof_vel_scale: float, + death_cost: float, + alive_reward_scale: float, + motor_effort_ratio: torch.Tensor, +): + heading_weight_tensor = torch.ones_like(heading_proj) * heading_weight + heading_reward = torch.where(heading_proj > 0.8, heading_weight_tensor, heading_weight * heading_proj / 0.8) + + # aligning up axis of robot and environment + up_reward = torch.zeros_like(heading_reward) + up_reward = torch.where(up_proj > 0.93, up_reward + up_weight, up_reward) + + # energy penalty for movement + actions_cost = torch.sum(actions**2, dim=-1) + electricity_cost = torch.sum( + torch.abs(actions * dof_vel * dof_vel_scale) * motor_effort_ratio.unsqueeze(0), + dim=-1, + ) + + # dof at limit cost + dof_at_limit_cost = torch.sum(dof_pos_scaled > 0.98, dim=-1) + + # reward for duration of staying alive + alive_reward = torch.ones_like(potentials) * alive_reward_scale + progress_reward = potentials - prev_potentials + + total_reward = ( + progress_reward + + alive_reward + + up_reward + + heading_reward + - actions_cost_scale * actions_cost + - energy_cost_scale * electricity_cost + - dof_at_limit_cost + ) + # adjust reward for fallen agents + total_reward = torch.where(reset_terminated, torch.ones_like(total_reward) * death_cost, total_reward) + return total_reward + + +@torch.jit.script +def compute_intermediate_values( + targets: torch.Tensor, + torso_position: torch.Tensor, + torso_rotation: torch.Tensor, + velocity: torch.Tensor, + ang_velocity: torch.Tensor, + dof_pos: torch.Tensor, + dof_lower_limits: torch.Tensor, + dof_upper_limits: torch.Tensor, + inv_start_rot: torch.Tensor, + basis_vec0: torch.Tensor, + basis_vec1: torch.Tensor, + potentials: torch.Tensor, + prev_potentials: torch.Tensor, + dt: float, +): + to_target = targets - torso_position + to_target[:, 2] = 0.0 + + torso_quat, up_proj, heading_proj, up_vec, heading_vec = compute_heading_and_up( + torso_rotation, inv_start_rot, to_target, basis_vec0, basis_vec1, 2 + ) + + vel_loc, angvel_loc, roll, pitch, yaw, angle_to_target = compute_rot( + torso_quat, velocity, ang_velocity, targets, torso_position + ) + + dof_pos_scaled = torch_utils.maths.unscale(dof_pos, dof_lower_limits, dof_upper_limits) + + to_target = targets - torso_position + to_target[:, 2] = 0.0 + prev_potentials[:] = potentials + potentials = -torch.norm(to_target, p=2, dim=-1) / dt + + return ( + up_proj, + heading_proj, + up_vec, + heading_vec, + vel_loc, + angvel_loc, + roll, + pitch, + yaw, + angle_to_target, + dof_pos_scaled, + prev_potentials, + potentials, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/__init__.py new file mode 100644 index 00000000000..0a0b44d58c3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Quacopter environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Quadcopter-Direct-v0", + entry_point=f"{__name__}.quadcopter_env:QuadcopterEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.quadcopter_env:QuadcopterEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:QuadcopterPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..193ed86f17f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [64, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: quadcopter_direct + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.016 + score_to_win: 20000 + max_epochs: 200 + save_best_after: 100 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..7552fbfd179 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class QuadcopterPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 200 + save_interval = 50 + experiment_name = "quadcopter_direct" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..c9df438bbbe --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "quadcopter_direct" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 4800 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/quadcopter_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/quadcopter_env.py new file mode 100644 index 00000000000..cebfc66a211 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/quadcopter/quadcopter_env.py @@ -0,0 +1,253 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import gymnasium as gym +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg +from isaaclab.envs.ui import BaseEnvWindow +from isaaclab.markers import VisualizationMarkers +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.math import subtract_frame_transforms + +## +# Pre-defined configs +## +from isaaclab_assets import CRAZYFLIE_CFG # isort: skip +from isaaclab.markers import CUBOID_MARKER_CFG # isort: skip + + +class QuadcopterEnvWindow(BaseEnvWindow): + """Window manager for the Quadcopter environment.""" + + def __init__(self, env: QuadcopterEnv, window_name: str = "IsaacLab"): + """Initialize the window. + + Args: + env: The environment object. + window_name: The name of the window. Defaults to "IsaacLab". + """ + # initialize base window + super().__init__(env, window_name) + # add custom UI elements + with self.ui_window_elements["main_vstack"]: + with self.ui_window_elements["debug_frame"]: + with self.ui_window_elements["debug_vstack"]: + # add command manager visualization + self._create_debug_vis_ui_element("targets", self.env) + + +@configclass +class QuadcopterEnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 10.0 + decimation = 2 + action_space = 4 + observation_space = 12 + state_space = 0 + debug_vis = True + + ui_window_class_type = QuadcopterEnvWindow + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 100, + render_interval=decimation, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + ) + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=2.5, replicate_physics=True) + + # robot + robot: ArticulationCfg = CRAZYFLIE_CFG.replace(prim_path="/World/envs/env_.*/Robot") + thrust_to_weight = 1.9 + moment_scale = 0.01 + + # reward scales + lin_vel_reward_scale = -0.05 + ang_vel_reward_scale = -0.01 + distance_to_goal_reward_scale = 15.0 + + +class QuadcopterEnv(DirectRLEnv): + cfg: QuadcopterEnvCfg + + def __init__(self, cfg: QuadcopterEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + # Total thrust and moment applied to the base of the quadcopter + self._actions = torch.zeros(self.num_envs, gym.spaces.flatdim(self.single_action_space), device=self.device) + self._thrust = torch.zeros(self.num_envs, 1, 3, device=self.device) + self._moment = torch.zeros(self.num_envs, 1, 3, device=self.device) + # Goal position + self._desired_pos_w = torch.zeros(self.num_envs, 3, device=self.device) + + # Logging + self._episode_sums = { + key: torch.zeros(self.num_envs, dtype=torch.float, device=self.device) + for key in [ + "lin_vel", + "ang_vel", + "distance_to_goal", + ] + } + # Get specific body indices + self._body_id = self._robot.find_bodies("body")[0] + self._robot_mass = self._robot.root_physx_view.get_masses()[0].sum() + self._gravity_magnitude = torch.tensor(self.sim.cfg.gravity, device=self.device).norm() + self._robot_weight = (self._robot_mass * self._gravity_magnitude).item() + + # add handle for debug visualization (this is set to a valid handle inside set_debug_vis) + self.set_debug_vis(self.cfg.debug_vis) + + def _setup_scene(self): + self._robot = Articulation(self.cfg.robot) + self.scene.articulations["robot"] = self._robot + + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self._terrain = self.cfg.terrain.class_type(self.cfg.terrain) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor): + self._actions = actions.clone().clamp(-1.0, 1.0) + self._thrust[:, 0, 2] = self.cfg.thrust_to_weight * self._robot_weight * (self._actions[:, 0] + 1.0) / 2.0 + self._moment[:, 0, :] = self.cfg.moment_scale * self._actions[:, 1:] + + def _apply_action(self): + self._robot.set_external_force_and_torque(self._thrust, self._moment, body_ids=self._body_id) + + def _get_observations(self) -> dict: + desired_pos_b, _ = subtract_frame_transforms( + self._robot.data.root_pos_w, self._robot.data.root_quat_w, self._desired_pos_w + ) + obs = torch.cat( + [ + self._robot.data.root_lin_vel_b, + self._robot.data.root_ang_vel_b, + self._robot.data.projected_gravity_b, + desired_pos_b, + ], + dim=-1, + ) + observations = {"policy": obs} + return observations + + def _get_rewards(self) -> torch.Tensor: + lin_vel = torch.sum(torch.square(self._robot.data.root_lin_vel_b), dim=1) + ang_vel = torch.sum(torch.square(self._robot.data.root_ang_vel_b), dim=1) + distance_to_goal = torch.linalg.norm(self._desired_pos_w - self._robot.data.root_pos_w, dim=1) + distance_to_goal_mapped = 1 - torch.tanh(distance_to_goal / 0.8) + rewards = { + "lin_vel": lin_vel * self.cfg.lin_vel_reward_scale * self.step_dt, + "ang_vel": ang_vel * self.cfg.ang_vel_reward_scale * self.step_dt, + "distance_to_goal": distance_to_goal_mapped * self.cfg.distance_to_goal_reward_scale * self.step_dt, + } + reward = torch.sum(torch.stack(list(rewards.values())), dim=0) + # Logging + for key, value in rewards.items(): + self._episode_sums[key] += value + return reward + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + time_out = self.episode_length_buf >= self.max_episode_length - 1 + died = torch.logical_or(self._robot.data.root_pos_w[:, 2] < 0.1, self._robot.data.root_pos_w[:, 2] > 2.0) + return died, time_out + + def _reset_idx(self, env_ids: torch.Tensor | None): + if env_ids is None or len(env_ids) == self.num_envs: + env_ids = self._robot._ALL_INDICES + + # Logging + final_distance_to_goal = torch.linalg.norm( + self._desired_pos_w[env_ids] - self._robot.data.root_pos_w[env_ids], dim=1 + ).mean() + extras = dict() + for key in self._episode_sums.keys(): + episodic_sum_avg = torch.mean(self._episode_sums[key][env_ids]) + extras["Episode_Reward/" + key] = episodic_sum_avg / self.max_episode_length_s + self._episode_sums[key][env_ids] = 0.0 + self.extras["log"] = dict() + self.extras["log"].update(extras) + extras = dict() + extras["Episode_Termination/died"] = torch.count_nonzero(self.reset_terminated[env_ids]).item() + extras["Episode_Termination/time_out"] = torch.count_nonzero(self.reset_time_outs[env_ids]).item() + extras["Metrics/final_distance_to_goal"] = final_distance_to_goal.item() + self.extras["log"].update(extras) + + self._robot.reset(env_ids) + super()._reset_idx(env_ids) + if len(env_ids) == self.num_envs: + # Spread out the resets to avoid spikes in training when many environments reset at a similar time + self.episode_length_buf = torch.randint_like(self.episode_length_buf, high=int(self.max_episode_length)) + + self._actions[env_ids] = 0.0 + # Sample new commands + self._desired_pos_w[env_ids, :2] = torch.zeros_like(self._desired_pos_w[env_ids, :2]).uniform_(-2.0, 2.0) + self._desired_pos_w[env_ids, :2] += self._terrain.env_origins[env_ids, :2] + self._desired_pos_w[env_ids, 2] = torch.zeros_like(self._desired_pos_w[env_ids, 2]).uniform_(0.5, 1.5) + # Reset robot state + joint_pos = self._robot.data.default_joint_pos[env_ids] + joint_vel = self._robot.data.default_joint_vel[env_ids] + default_root_state = self._robot.data.default_root_state[env_ids] + default_root_state[:, :3] += self._terrain.env_origins[env_ids] + self._robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids) + self._robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids) + self._robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + def _set_debug_vis_impl(self, debug_vis: bool): + # create markers if necessary for the first tome + if debug_vis: + if not hasattr(self, "goal_pos_visualizer"): + marker_cfg = CUBOID_MARKER_CFG.copy() + marker_cfg.markers["cuboid"].size = (0.05, 0.05, 0.05) + # -- goal pose + marker_cfg.prim_path = "/Visuals/Command/goal_position" + self.goal_pos_visualizer = VisualizationMarkers(marker_cfg) + # set their visibility to true + self.goal_pos_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_pos_visualizer"): + self.goal_pos_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # update the markers + self.goal_pos_visualizer.visualize(self._desired_pos_w) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/__init__.py new file mode 100644 index 00000000000..d2e5cf3a7ec --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/__init__.py @@ -0,0 +1,81 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Shadow Hand environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +inhand_task_entry = "isaaclab_tasks.direct.inhand_manipulation" + +gym.register( + id="Isaac-Repose-Cube-Shadow-Direct-v0", + entry_point=f"{inhand_task_entry}.inhand_manipulation_env:InHandManipulationEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0", + entry_point=f"{inhand_task_entry}.inhand_manipulation_env:InHandManipulationEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandOpenAIEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_ff_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandAsymFFPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ff_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0", + entry_point=f"{inhand_task_entry}.inhand_manipulation_env:InHandManipulationEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandOpenAIEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_lstm_cfg.yaml", + }, +) + +### Vision + +gym.register( + id="Isaac-Repose-Cube-Shadow-Vision-Direct-v0", + entry_point=f"{__name__}.shadow_hand_vision_env:ShadowHandVisionEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_vision_env:ShadowHandVisionEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandVisionFFPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_vision_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Repose-Cube-Shadow-Vision-Direct-Play-v0", + entry_point=f"{__name__}.shadow_hand_vision_env:ShadowHandVisionEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_vision_env:ShadowHandVisionEnvPlayCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandVisionFFPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_vision_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..8f18adc4bd0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,86 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512, 512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: shadow_hand + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 5000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml new file mode 100644 index 00000000000..454406497a5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_ff_cfg.yaml @@ -0,0 +1,107 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [400, 400, 200, 100] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: shadow_hand_openai_ff + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.998 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 10000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 16384 + mini_epochs: 4 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + central_value_config: + minibatch_size: 32768 + mini_epochs: 4 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + clip_value: True + normalize_input: True + truncate_grads: True + + network: + name: actor_critic + central_value: True + mlp: + units: [512, 512, 256, 128] + activation: elu + d2rl: False + initializer: + name: default + regularizer: + name: None + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml new file mode 100644 index 00000000000..c404d96d9f4 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_lstm_cfg.yaml @@ -0,0 +1,118 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512] + activation: relu + d2rl: False + + initializer: + name: default + regularizer: + name: None + rnn: + name: lstm + units: 1024 + layers: 1 + before_mlp: True + layer_norm: True + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: shadow_hand_openai_lstm + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.998 + tau: 0.95 + learning_rate: 1e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 10000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 16384 + mini_epochs: 4 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + central_value_config: + minibatch_size: 32768 + mini_epochs: 4 + learning_rate: 1e-4 + kl_threshold: 0.016 + clip_value: True + normalize_input: True + truncate_grads: True + + network: + name: actor_critic + central_value: True + mlp: + units: [512] + activation: relu + d2rl: False + initializer: + name: default + regularizer: + name: None + rnn: + name: lstm + units: 1024 + layers: 1 + before_mlp: True + layer_norm: True + zero_rnn_on_done: False + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml new file mode 100644 index 00000000000..f77e6cc98ac --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rl_games_ppo_vision_cfg.yaml @@ -0,0 +1,107 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [1024, 512, 512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: shadow_hand_vision + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.01 + score_to_win: 100000 + max_epochs: 50000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 64 + minibatch_size: 19600 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + central_value_config: + minibatch_size: 19600 + mini_epochs: 5 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.01 + clip_value: True + normalize_input: True + truncate_grads: True + + network: + name: actor_critic + central_value: True + mlp: + units: [1024, 512, 512, 256, 128] + activation: elu + d2rl: False + initializer: + name: default + regularizer: + name: None + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..577b9a90752 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,100 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class ShadowHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 10000 + save_interval = 250 + experiment_name = "shadow_hand" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 512, 256, 128], + critic_hidden_dims=[512, 512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.016, + max_grad_norm=1.0, + ) + + +@configclass +class ShadowHandAsymFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 10000 + save_interval = 250 + experiment_name = "shadow_hand_openai_ff" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[400, 400, 200, 100], + critic_hidden_dims=[512, 512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=4, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class ShadowHandVisionFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 64 + max_iterations = 50000 + save_interval = 250 + experiment_name = "shadow_hand_vision" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[1024, 512, 512, 256, 128], + critic_hidden_dims=[1024, 512, 512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml new file mode 100644 index 00000000000..c977ab4b660 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [400, 400, 200, 100] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 4 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "shadow_hand_openai_ff" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 160000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..37a635e669b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "shadow_hand" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 80000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/feature_extractor.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/feature_extractor.py new file mode 100644 index 00000000000..0dad6ef2c84 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/feature_extractor.py @@ -0,0 +1,195 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import glob +import os +import torch +import torch.nn as nn +import torchvision + +from isaaclab.sensors import save_images_to_file +from isaaclab.utils import configclass + + +class FeatureExtractorNetwork(nn.Module): + """CNN architecture used to regress keypoint positions of the in-hand cube from image data.""" + + def __init__(self): + super().__init__() + num_channel = 7 + self.cnn = nn.Sequential( + nn.Conv2d(num_channel, 16, kernel_size=6, stride=2, padding=0), + nn.ReLU(), + nn.LayerNorm([16, 58, 58]), + nn.Conv2d(16, 32, kernel_size=4, stride=2, padding=0), + nn.ReLU(), + nn.LayerNorm([32, 28, 28]), + nn.Conv2d(32, 64, kernel_size=4, stride=2, padding=0), + nn.ReLU(), + nn.LayerNorm([64, 13, 13]), + nn.Conv2d(64, 128, kernel_size=3, stride=2, padding=0), + nn.ReLU(), + nn.LayerNorm([128, 6, 6]), + nn.AvgPool2d(6), + ) + + self.linear = nn.Sequential( + nn.Linear(128, 27), + ) + + self.data_transforms = torchvision.transforms.Compose([ + torchvision.transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]), + ]) + + def forward(self, x): + x = x.permute(0, 3, 1, 2) + x[:, 0:3, :, :] = self.data_transforms(x[:, 0:3, :, :]) + x[:, 4:7, :, :] = self.data_transforms(x[:, 4:7, :, :]) + cnn_x = self.cnn(x) + out = self.linear(cnn_x.view(-1, 128)) + return out + + +@configclass +class FeatureExtractorCfg: + """Configuration for the feature extractor model.""" + + train: bool = True + """If True, the feature extractor model is trained during the rollout process. Default is False.""" + + load_checkpoint: bool = False + """If True, the feature extractor model is loaded from a checkpoint. Default is False.""" + + write_image_to_file: bool = False + """If True, the images from the camera sensor are written to file. Default is False.""" + + +class FeatureExtractor: + """Class for extracting features from image data. + + It uses a CNN to regress keypoint positions from normalized RGB, depth, and segmentation images. + If the train flag is set to True, the CNN is trained during the rollout process. + """ + + def __init__(self, cfg: FeatureExtractorCfg, device: str): + """Initialize the feature extractor model. + + Args: + cfg (FeatureExtractorCfg): Configuration for the feature extractor model. + device (str): Device to run the model on. + """ + + self.cfg = cfg + self.device = device + + # Feature extractor model + self.feature_extractor = FeatureExtractorNetwork() + self.feature_extractor.to(self.device) + + self.step_count = 0 + self.log_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "logs") + if not os.path.exists(self.log_dir): + os.makedirs(self.log_dir) + + if self.cfg.load_checkpoint: + list_of_files = glob.glob(self.log_dir + "/*.pth") + latest_file = max(list_of_files, key=os.path.getctime) + checkpoint = os.path.join(self.log_dir, latest_file) + print(f"[INFO]: Loading feature extractor checkpoint from {checkpoint}") + self.feature_extractor.load_state_dict(torch.load(checkpoint, weights_only=True)) + + if self.cfg.train: + self.optimizer = torch.optim.Adam(self.feature_extractor.parameters(), lr=1e-4) + self.l2_loss = nn.MSELoss() + self.feature_extractor.train() + else: + self.feature_extractor.eval() + + def _preprocess_images( + self, rgb_img: torch.Tensor, depth_img: torch.Tensor, segmentation_img: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Preprocesses the input images. + + Args: + rgb_img (torch.Tensor): RGB image tensor. Shape: (N, H, W, 3). + depth_img (torch.Tensor): Depth image tensor. Shape: (N, H, W, 1). + segmentation_img (torch.Tensor): Segmentation image tensor. Shape: (N, H, W, 3) + + Returns: + tuple[torch.Tensor, torch.Tensor, torch.Tensor]: Preprocessed RGB, depth, and segmentation + """ + rgb_img = rgb_img / 255.0 + # process depth image + depth_img[depth_img == float("inf")] = 0 + depth_img /= 5.0 + depth_img /= torch.max(depth_img) + # process segmentation image + segmentation_img = segmentation_img / 255.0 + mean_tensor = torch.mean(segmentation_img, dim=(1, 2), keepdim=True) + segmentation_img -= mean_tensor + return rgb_img, depth_img, segmentation_img + + def _save_images(self, rgb_img: torch.Tensor, depth_img: torch.Tensor, segmentation_img: torch.Tensor): + """Writes image buffers to file. + + Args: + rgb_img (torch.Tensor): RGB image tensor. Shape: (N, H, W, 3). + depth_img (torch.Tensor): Depth image tensor. Shape: (N, H, W, 1). + segmentation_img (torch.Tensor): Segmentation image tensor. Shape: (N, H, W, 3). + """ + save_images_to_file(rgb_img, "shadow_hand_rgb.png") + save_images_to_file(depth_img, "shadow_hand_depth.png") + save_images_to_file(segmentation_img, "shadow_hand_segmentation.png") + + def step( + self, rgb_img: torch.Tensor, depth_img: torch.Tensor, segmentation_img: torch.Tensor, gt_pose: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor]: + """Extracts the features using the images and trains the model if the train flag is set to True. + + Args: + rgb_img (torch.Tensor): RGB image tensor. Shape: (N, H, W, 3). + depth_img (torch.Tensor): Depth image tensor. Shape: (N, H, W, 1). + segmentation_img (torch.Tensor): Segmentation image tensor. Shape: (N, H, W, 3). + gt_pose (torch.Tensor): Ground truth pose tensor (position and corners). Shape: (N, 27). + + Returns: + tuple[torch.Tensor, torch.Tensor]: Pose loss and predicted pose. + """ + + rgb_img, depth_img, segmentation_img = self._preprocess_images(rgb_img, depth_img, segmentation_img) + + if self.cfg.write_image_to_file: + self._save_images(rgb_img, depth_img, segmentation_img) + + if self.cfg.train: + with torch.enable_grad(): + with torch.inference_mode(False): + img_input = torch.cat((rgb_img, depth_img, segmentation_img), dim=-1) + self.optimizer.zero_grad() + + predicted_pose = self.feature_extractor(img_input) + pose_loss = self.l2_loss(predicted_pose, gt_pose.clone()) * 100 + + pose_loss.backward() + self.optimizer.step() + + if self.step_count % 50000 == 0: + torch.save( + self.feature_extractor.state_dict(), + os.path.join(self.log_dir, f"cnn_{self.step_count}_{pose_loss.detach().cpu().numpy()}.pth"), + ) + + self.step_count += 1 + + return pose_loss, predicted_pose + else: + img_input = torch.cat((rgb_img, depth_img, segmentation_img), dim=-1) + predicted_pose = self.feature_extractor(img_input) + return None, predicted_pose diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py new file mode 100644 index 00000000000..2006f3bf1bd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py @@ -0,0 +1,288 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG + +import isaaclab.envs.mdp as mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import GaussianNoiseCfg, NoiseModelWithAdditiveBiasCfg + + +@configclass +class EventCfg: + """Configuration for randomization.""" + + # -- robot + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="reset", + min_step_count_between_reset=720, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (1.0, 1.0), + "restitution_range": (1.0, 1.0), + "num_buckets": 250, + }, + ) + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "stiffness_distribution_params": (0.75, 1.5), + "damping_distribution_params": (0.3, 3.0), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + robot_joint_pos_limits = EventTerm( + func=mdp.randomize_joint_parameters, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "lower_limit_distribution_params": (0.00, 0.01), + "upper_limit_distribution_params": (0.00, 0.01), + "operation": "add", + "distribution": "gaussian", + }, + ) + robot_tendon_properties = EventTerm( + func=mdp.randomize_fixed_tendon_parameters, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", fixed_tendon_names=".*"), + "stiffness_distribution_params": (0.75, 1.5), + "damping_distribution_params": (0.3, 3.0), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + # -- object + object_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("object"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (1.0, 1.0), + "restitution_range": (1.0, 1.0), + "num_buckets": 250, + }, + ) + object_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("object"), + "mass_distribution_params": (0.5, 1.5), + "operation": "scale", + "distribution": "uniform", + }, + ) + + # -- scene + reset_gravity = EventTerm( + func=mdp.randomize_physics_scene_gravity, + mode="interval", + is_global_time=True, + interval_range_s=(36.0, 36.0), # time_s = num_steps * (decimation * dt) + params={ + "gravity_distribution_params": ([0.0, 0.0, 0.0], [0.0, 0.0, 0.4]), + "operation": "add", + "distribution": "gaussian", + }, + ) + + +@configclass +class ShadowHandEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 10.0 + action_space = 20 + observation_space = 157 # (full) + state_space = 0 + asymmetric_obs = False + obs_type = "full" + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 120, + render_interval=decimation, + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + physx=PhysxCfg( + bounce_threshold_velocity=0.2, + ), + ) + # robot + robot_cfg: ArticulationCfg = SHADOW_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot").replace( + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={".*": 0.0}, + ) + ) + actuated_joint_names = [ + "robot0_WRJ1", + "robot0_WRJ0", + "robot0_FFJ3", + "robot0_FFJ2", + "robot0_FFJ1", + "robot0_MFJ3", + "robot0_MFJ2", + "robot0_MFJ1", + "robot0_RFJ3", + "robot0_RFJ2", + "robot0_RFJ1", + "robot0_LFJ4", + "robot0_LFJ3", + "robot0_LFJ2", + "robot0_LFJ1", + "robot0_THJ4", + "robot0_THJ3", + "robot0_THJ2", + "robot0_THJ1", + "robot0_THJ0", + ] + fingertip_body_names = [ + "robot0_ffdistal", + "robot0_mfdistal", + "robot0_rfdistal", + "robot0_lfdistal", + "robot0_thdistal", + ] + + # in-hand object + object_cfg: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + mass_props=sim_utils.MassPropertiesCfg(density=567.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.6), rot=(1.0, 0.0, 0.0, 0.0)), + ) + # goal object + goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(1.0, 1.0, 1.0), + ) + }, + ) + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=8192, env_spacing=0.75, replicate_physics=True) + + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # reward scales + dist_reward_scale = -10.0 + rot_reward_scale = 1.0 + rot_eps = 0.1 + action_penalty_scale = -0.0002 + reach_goal_bonus = 250 + fall_penalty = 0 + fall_dist = 0.24 + vel_obs_scale = 0.2 + success_tolerance = 0.1 + max_consecutive_success = 0 + av_factor = 0.1 + act_moving_average = 1.0 + force_torque_obs_scale = 10.0 + + +@configclass +class ShadowHandOpenAIEnvCfg(ShadowHandEnvCfg): + # env + decimation = 3 + episode_length_s = 8.0 + action_space = 20 + observation_space = 42 + state_space = 187 + asymmetric_obs = True + obs_type = "openai" + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + render_interval=decimation, + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + physx=PhysxCfg( + bounce_threshold_velocity=0.2, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + ), + ) + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # reward scales + dist_reward_scale = -10.0 + rot_reward_scale = 1.0 + rot_eps = 0.1 + action_penalty_scale = -0.0002 + reach_goal_bonus = 250 + fall_penalty = -50 + fall_dist = 0.24 + vel_obs_scale = 0.2 + success_tolerance = 0.4 + max_consecutive_success = 50 + av_factor = 0.1 + act_moving_average = 0.3 + force_torque_obs_scale = 10.0 + # domain randomization config + events: EventCfg = EventCfg() + # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset + action_noise_model: NoiseModelWithAdditiveBiasCfg = NoiseModelWithAdditiveBiasCfg( + noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.05, operation="add"), + bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.015, operation="abs"), + ) + # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset + observation_noise_model: NoiseModelWithAdditiveBiasCfg = NoiseModelWithAdditiveBiasCfg( + noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.002, operation="add"), + bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.0001, operation="abs"), + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py new file mode 100644 index 00000000000..70843ff2a48 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py @@ -0,0 +1,208 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import torch + +import omni.usd + +# from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated +try: + import Semantics +except ModuleNotFoundError: + from pxr import Semantics + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import TiledCamera, TiledCameraCfg +from isaaclab.utils import configclass +from isaaclab.utils.math import quat_apply + +from isaaclab_tasks.direct.inhand_manipulation.inhand_manipulation_env import InHandManipulationEnv, unscale + +from .feature_extractor import FeatureExtractor, FeatureExtractorCfg +from .shadow_hand_env_cfg import ShadowHandEnvCfg + + +@configclass +class ShadowHandVisionEnvCfg(ShadowHandEnvCfg): + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1225, env_spacing=2.0, replicate_physics=True) + + # camera + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="/World/envs/env_.*/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(0, -0.35, 1.0), rot=(0.7071, 0.0, 0.7071, 0.0), convention="world"), + data_types=["rgb", "depth", "semantic_segmentation"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=120, + height=120, + ) + feature_extractor = FeatureExtractorCfg() + + # env + observation_space = 164 + 27 # state observation + vision CNN embedding + state_space = 187 + 27 # asymettric states + vision CNN embedding + + +@configclass +class ShadowHandVisionEnvPlayCfg(ShadowHandVisionEnvCfg): + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=64, env_spacing=2.0, replicate_physics=True) + # inference for CNN + feature_extractor = FeatureExtractorCfg(train=False, load_checkpoint=True) + + +class ShadowHandVisionEnv(InHandManipulationEnv): + cfg: ShadowHandVisionEnvCfg + + def __init__(self, cfg: ShadowHandVisionEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + self.feature_extractor = FeatureExtractor(self.cfg.feature_extractor, self.device) + # hide goal cubes + self.goal_pos[:, :] = torch.tensor([-0.2, 0.1, 0.6], device=self.device) + # keypoints buffer + self.gt_keypoints = torch.ones(self.num_envs, 8, 3, dtype=torch.float32, device=self.device) + self.goal_keypoints = torch.ones(self.num_envs, 8, 3, dtype=torch.float32, device=self.device) + + def _setup_scene(self): + # add hand, in-hand object, and goal object + self.hand = Articulation(self.cfg.robot_cfg) + self.object = RigidObject(self.cfg.object_cfg) + self._tiled_camera = TiledCamera(self.cfg.tiled_camera) + # get stage + stage = omni.usd.get_context().get_stage() + # add semantics for in-hand cube + prim = stage.GetPrimAtPath("/World/envs/env_0/object") + sem = Semantics.SemanticsAPI.Apply(prim, "Semantics") + sem.CreateSemanticTypeAttr() + sem.CreateSemanticDataAttr() + sem.GetSemanticTypeAttr().Set("class") + sem.GetSemanticDataAttr().Set("cube") + # clone and replicate (no need to filter for this environment) + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene - we must register to scene to randomize with EventManager + self.scene.articulations["robot"] = self.hand + self.scene.rigid_objects["object"] = self.object + self.scene.sensors["tiled_camera"] = self._tiled_camera + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _compute_image_observations(self): + # generate ground truth keypoints for in-hand cube + compute_keypoints(pose=torch.cat((self.object_pos, self.object_rot), dim=1), out=self.gt_keypoints) + + object_pose = torch.cat([self.object_pos, self.gt_keypoints.view(-1, 24)], dim=-1) + + # train CNN to regress on keypoint positions + pose_loss, embeddings = self.feature_extractor.step( + self._tiled_camera.data.output["rgb"], + self._tiled_camera.data.output["depth"], + self._tiled_camera.data.output["semantic_segmentation"][..., :3], + object_pose, + ) + + self.embeddings = embeddings.clone().detach() + # compute keypoints for goal cube + compute_keypoints( + pose=torch.cat((torch.zeros_like(self.goal_pos), self.goal_rot), dim=-1), out=self.goal_keypoints + ) + + obs = torch.cat( + ( + self.embeddings, + self.goal_keypoints.view(-1, 24), + ), + dim=-1, + ) + + # log pose loss from CNN training + if "log" not in self.extras: + self.extras["log"] = dict() + self.extras["log"]["pose_loss"] = pose_loss + + return obs + + def _compute_proprio_observations(self): + """Proprioception observations from physics.""" + obs = torch.cat( + ( + # hand + unscale(self.hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + self.cfg.vel_obs_scale * self.hand_dof_vel, + # goal + self.in_hand_pos, + self.goal_rot, + # fingertips + self.fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + self.fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + self.fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # actions + self.actions, + ), + dim=-1, + ) + return obs + + def _compute_states(self): + """Asymmetric states for the critic.""" + sim_states = self.compute_full_state() + state = torch.cat((sim_states, self.embeddings), dim=-1) + return state + + def _get_observations(self) -> dict: + # proprioception observations + state_obs = self._compute_proprio_observations() + # vision observations from CMM + image_obs = self._compute_image_observations() + obs = torch.cat((state_obs, image_obs), dim=-1) + # asymmetric critic states + self.fingertip_force_sensors = self.hand.root_physx_view.get_link_incoming_joint_force()[:, self.finger_bodies] + state = self._compute_states() + + observations = {"policy": obs, "critic": state} + return observations + + +@torch.jit.script +def compute_keypoints( + pose: torch.Tensor, + num_keypoints: int = 8, + size: tuple[float, float, float] = (2 * 0.03, 2 * 0.03, 2 * 0.03), + out: torch.Tensor | None = None, +): + """Computes positions of 8 corner keypoints of a cube. + + Args: + pose: Position and orientation of the center of the cube. Shape is (N, 7) + num_keypoints: Number of keypoints to compute. Default = 8 + size: Length of X, Y, Z dimensions of cube. Default = [0.06, 0.06, 0.06] + out: Buffer to store keypoints. If None, a new buffer will be created. + """ + num_envs = pose.shape[0] + if out is None: + out = torch.ones(num_envs, num_keypoints, 3, dtype=torch.float32, device=pose.device) + else: + out[:] = 1.0 + for i in range(num_keypoints): + # which dimensions to negate + n = [((i >> k) & 1) == 0 for k in range(3)] + corner_loc = ([(1 if n[k] else -1) * s / 2 for k, s in enumerate(size)],) + corner = torch.tensor(corner_loc, dtype=torch.float32, device=pose.device) * out[:, i, :] + # express corner position in the world frame + out[:, i, :] = pose[:, :3] + quat_apply(pose[:, 3:7], corner) + + return out diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/__init__.py new file mode 100644 index 00000000000..b9cc043377d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +ShadowHand Over environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Shadow-Hand-Over-Direct-v0", + entry_point=f"{__name__}.shadow_hand_over_env:ShadowHandOverEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.shadow_hand_over_env_cfg:ShadowHandOverEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "skrl_ippo_cfg_entry_point": f"{agents.__name__}:skrl_ippo_cfg.yaml", + "skrl_mappo_cfg_entry_point": f"{agents.__name__}:skrl_mappo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..daa9060d689 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,86 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512, 512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: shadow_hand_over + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 5000 + save_best_after: 100 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 + + player: + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml new file mode 100644 index 00000000000..18cde08c4ed --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# IPPO agent configuration (field names are from IPPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/multi_agents/ippo.html +agent: + class: IPPO + rollouts: 16 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "shadow_hand_over" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml new file mode 100644 index 00000000000..f67cc31b249 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml @@ -0,0 +1,82 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: True + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# MAPPO agent configuration (field names are from MAPPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/multi_agents/mappo.html +agent: + class: MAPPO + rollouts: 16 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + shared_state_preprocessor: RunningStandardScaler + shared_state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "shadow_hand_over" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..9cfc4b64f20 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "shadow_hand_over" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py new file mode 100644 index 00000000000..d775431b7ed --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py @@ -0,0 +1,428 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import numpy as np +import torch +from collections.abc import Sequence + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs import DirectMARLEnv +from isaaclab.markers import VisualizationMarkers +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.utils.math import quat_conjugate, quat_from_angle_axis, quat_mul, sample_uniform, saturate + +from .shadow_hand_over_env_cfg import ShadowHandOverEnvCfg + + +class ShadowHandOverEnv(DirectMARLEnv): + cfg: ShadowHandOverEnvCfg + + def __init__(self, cfg: ShadowHandOverEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + self.num_hand_dofs = self.right_hand.num_joints + + # buffers for position targets + self.right_hand_dof_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + self.right_hand_prev_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + self.right_hand_curr_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + self.left_hand_dof_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + self.left_hand_prev_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + self.left_hand_curr_targets = torch.zeros( + (self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device + ) + + # list of actuated joints + self.actuated_dof_indices = list() + for joint_name in cfg.actuated_joint_names: + self.actuated_dof_indices.append(self.right_hand.joint_names.index(joint_name)) + self.actuated_dof_indices.sort() + + # finger bodies + self.finger_bodies = list() + for body_name in self.cfg.fingertip_body_names: + self.finger_bodies.append(self.right_hand.body_names.index(body_name)) + self.finger_bodies.sort() + self.num_fingertips = len(self.finger_bodies) + + # joint limits + joint_pos_limits = self.right_hand.root_physx_view.get_dof_limits().to(self.device) + self.hand_dof_lower_limits = joint_pos_limits[..., 0] + self.hand_dof_upper_limits = joint_pos_limits[..., 1] + + # used to compare object position + self.in_hand_pos = self.object.data.default_root_state[:, 0:3].clone() + self.in_hand_pos[:, 2] -= 0.04 + # default goal positions + self.goal_rot = torch.zeros((self.num_envs, 4), dtype=torch.float, device=self.device) + self.goal_rot[:, 0] = 1.0 + self.goal_pos = torch.zeros((self.num_envs, 3), dtype=torch.float, device=self.device) + self.goal_pos[:, :] = torch.tensor([0.0, -0.64, 0.54], device=self.device) + # initialize goal marker + self.goal_markers = VisualizationMarkers(self.cfg.goal_object_cfg) + + # unit tensors + self.x_unit_tensor = torch.tensor([1, 0, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + self.y_unit_tensor = torch.tensor([0, 1, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + self.z_unit_tensor = torch.tensor([0, 0, 1], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) + + def _setup_scene(self): + # add hand, in-hand object, and goal object + self.right_hand = Articulation(self.cfg.right_robot_cfg) + self.left_hand = Articulation(self.cfg.left_robot_cfg) + self.object = RigidObject(self.cfg.object_cfg) + # add ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + # clone and replicate (no need to filter for this environment) + self.scene.clone_environments(copy_from_source=False) + # add articulation to scene - we must register to scene to randomize with EventManager + self.scene.articulations["right_robot"] = self.right_hand + self.scene.articulations["left_robot"] = self.left_hand + self.scene.rigid_objects["object"] = self.object + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: dict[str, torch.Tensor]) -> None: + self.actions = actions + + def _apply_action(self) -> None: + # right hand target + self.right_hand_curr_targets[:, self.actuated_dof_indices] = scale( + self.actions["right_hand"], + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + self.right_hand_curr_targets[:, self.actuated_dof_indices] = ( + self.cfg.act_moving_average * self.right_hand_curr_targets[:, self.actuated_dof_indices] + + (1.0 - self.cfg.act_moving_average) * self.right_hand_prev_targets[:, self.actuated_dof_indices] + ) + self.right_hand_curr_targets[:, self.actuated_dof_indices] = saturate( + self.right_hand_curr_targets[:, self.actuated_dof_indices], + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + + # left hand target + self.left_hand_curr_targets[:, self.actuated_dof_indices] = scale( + self.actions["left_hand"], + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + self.left_hand_curr_targets[:, self.actuated_dof_indices] = ( + self.cfg.act_moving_average * self.left_hand_curr_targets[:, self.actuated_dof_indices] + + (1.0 - self.cfg.act_moving_average) * self.left_hand_prev_targets[:, self.actuated_dof_indices] + ) + self.left_hand_curr_targets[:, self.actuated_dof_indices] = saturate( + self.left_hand_curr_targets[:, self.actuated_dof_indices], + self.hand_dof_lower_limits[:, self.actuated_dof_indices], + self.hand_dof_upper_limits[:, self.actuated_dof_indices], + ) + + # save current targets + self.right_hand_prev_targets[:, self.actuated_dof_indices] = self.right_hand_curr_targets[ + :, self.actuated_dof_indices + ] + self.left_hand_prev_targets[:, self.actuated_dof_indices] = self.left_hand_curr_targets[ + :, self.actuated_dof_indices + ] + + # set targets + self.right_hand.set_joint_position_target( + self.right_hand_curr_targets[:, self.actuated_dof_indices], joint_ids=self.actuated_dof_indices + ) + self.left_hand.set_joint_position_target( + self.left_hand_curr_targets[:, self.actuated_dof_indices], joint_ids=self.actuated_dof_indices + ) + + def _get_observations(self) -> dict[str, torch.Tensor]: + observations = { + "right_hand": torch.cat( + ( + # ---- right hand ---- + # DOF positions (24) + unscale(self.right_hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + # DOF velocities (24) + self.cfg.vel_obs_scale * self.right_hand_dof_vel, + # fingertip positions (5 * 3) + self.right_fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + # fingertip rotations (5 * 4) + self.right_fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + # fingertip linear and angular velocities (5 * 6) + self.right_fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # applied actions (20) + self.actions["right_hand"], + # ---- object ---- + # positions (3) + self.object_pos, + # rotations (4) + self.object_rot, + # linear velocities (3) + self.object_linvel, + # angular velocities (3) + self.cfg.vel_obs_scale * self.object_angvel, + # ---- goal ---- + # positions (3) + self.goal_pos, + # rotations (4) + self.goal_rot, + # goal-object rotation diff (4) + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + ), + dim=-1, + ), + "left_hand": torch.cat( + ( + # ---- left hand ---- + # DOF positions (24) + unscale(self.left_hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + # DOF velocities (24) + self.cfg.vel_obs_scale * self.left_hand_dof_vel, + # fingertip positions (5 * 3) + self.left_fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + # fingertip rotations (5 * 4) + self.left_fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + # fingertip linear and angular velocities (5 * 6) + self.left_fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # applied actions (20) + self.actions["left_hand"], + # ---- object ---- + # positions (3) + self.object_pos, + # rotations (4) + self.object_rot, + # linear velocities (3) + self.object_linvel, + # angular velocities (3) + self.cfg.vel_obs_scale * self.object_angvel, + # ---- goal ---- + # positions (3) + self.goal_pos, + # rotations (4) + self.goal_rot, + # goal-object rotation diff (4) + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + ), + dim=-1, + ), + } + return observations + + def _get_states(self) -> torch.Tensor: + states = torch.cat( + ( + # ---- right hand ---- + # DOF positions (24) + unscale(self.right_hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + # DOF velocities (24) + self.cfg.vel_obs_scale * self.right_hand_dof_vel, + # fingertip positions (5 * 3) + self.right_fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + # fingertip rotations (5 * 4) + self.right_fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + # fingertip linear and angular velocities (5 * 6) + self.right_fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # applied actions (20) + self.actions["right_hand"], + # ---- left hand ---- + # DOF positions (24) + unscale(self.left_hand_dof_pos, self.hand_dof_lower_limits, self.hand_dof_upper_limits), + # DOF velocities (24) + self.cfg.vel_obs_scale * self.left_hand_dof_vel, + # fingertip positions (5 * 3) + self.left_fingertip_pos.view(self.num_envs, self.num_fingertips * 3), + # fingertip rotations (5 * 4) + self.left_fingertip_rot.view(self.num_envs, self.num_fingertips * 4), + # fingertip linear and angular velocities (5 * 6) + self.left_fingertip_velocities.view(self.num_envs, self.num_fingertips * 6), + # applied actions (20) + self.actions["left_hand"], + # ---- object ---- + # positions (3) + self.object_pos, + # rotations (4) + self.object_rot, + # linear velocities (3) + self.object_linvel, + # angular velocities (3) + self.cfg.vel_obs_scale * self.object_angvel, + # ---- goal ---- + # positions (3) + self.goal_pos, + # rotations (4) + self.goal_rot, + # goal-object rotation diff (4) + quat_mul(self.object_rot, quat_conjugate(self.goal_rot)), + ), + dim=-1, + ) + return states + + def _get_rewards(self) -> dict[str, torch.Tensor]: + # compute reward + goal_dist = torch.norm(self.object_pos - self.goal_pos, p=2, dim=-1) + rew_dist = 2 * torch.exp(-self.cfg.dist_reward_scale * goal_dist) + + # log reward components + if "log" not in self.extras: + self.extras["log"] = dict() + self.extras["log"]["dist_reward"] = rew_dist.mean() + self.extras["log"]["dist_goal"] = goal_dist.mean() + + return {"right_hand": rew_dist, "left_hand": rew_dist} + + def _get_dones(self) -> tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: + self._compute_intermediate_values() + + # reset when object has fallen + out_of_reach = self.object_pos[:, 2] <= self.cfg.fall_dist + # reset when episode ends + time_out = self.episode_length_buf >= self.max_episode_length - 1 + + terminated = {agent: out_of_reach for agent in self.cfg.possible_agents} + time_outs = {agent: time_out for agent in self.cfg.possible_agents} + return terminated, time_outs + + def _reset_idx(self, env_ids: Sequence[int] | torch.Tensor | None): + if env_ids is None: + env_ids = self.right_hand._ALL_INDICES + # reset articulation and rigid body attributes + super()._reset_idx(env_ids) + + # reset goals + self._reset_target_pose(env_ids) + + # reset object + object_default_state = self.object.data.default_root_state.clone()[env_ids] + pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), 3), device=self.device) + + object_default_state[:, 0:3] = ( + object_default_state[:, 0:3] + self.cfg.reset_position_noise * pos_noise + self.scene.env_origins[env_ids] + ) + + rot_noise = sample_uniform(-1.0, 1.0, (len(env_ids), 2), device=self.device) # noise for X and Y rotation + object_default_state[:, 3:7] = randomize_rotation( + rot_noise[:, 0], rot_noise[:, 1], self.x_unit_tensor[env_ids], self.y_unit_tensor[env_ids] + ) + + object_default_state[:, 7:] = torch.zeros_like(self.object.data.default_root_state[env_ids, 7:]) + self.object.write_root_pose_to_sim(object_default_state[:, :7], env_ids) + self.object.write_root_velocity_to_sim(object_default_state[:, 7:], env_ids) + + # reset right hand + delta_max = self.hand_dof_upper_limits[env_ids] - self.right_hand.data.default_joint_pos[env_ids] + delta_min = self.hand_dof_lower_limits[env_ids] - self.right_hand.data.default_joint_pos[env_ids] + + dof_pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + rand_delta = delta_min + (delta_max - delta_min) * 0.5 * dof_pos_noise + dof_pos = self.right_hand.data.default_joint_pos[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + + dof_vel_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + dof_vel = self.right_hand.data.default_joint_vel[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise + + self.right_hand_prev_targets[env_ids] = dof_pos + self.right_hand_curr_targets[env_ids] = dof_pos + self.right_hand_dof_targets[env_ids] = dof_pos + + self.right_hand.set_joint_position_target(dof_pos, env_ids=env_ids) + self.right_hand.write_joint_state_to_sim(dof_pos, dof_vel, env_ids=env_ids) + + # reset left hand + delta_max = self.hand_dof_upper_limits[env_ids] - self.left_hand.data.default_joint_pos[env_ids] + delta_min = self.hand_dof_lower_limits[env_ids] - self.left_hand.data.default_joint_pos[env_ids] + + dof_pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + rand_delta = delta_min + (delta_max - delta_min) * 0.5 * dof_pos_noise + dof_pos = self.left_hand.data.default_joint_pos[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + + dof_vel_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) + dof_vel = self.left_hand.data.default_joint_vel[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise + + self.left_hand_prev_targets[env_ids] = dof_pos + self.left_hand_curr_targets[env_ids] = dof_pos + self.left_hand_dof_targets[env_ids] = dof_pos + + self.left_hand.set_joint_position_target(dof_pos, env_ids=env_ids) + self.left_hand.write_joint_state_to_sim(dof_pos, dof_vel, env_ids=env_ids) + + self._compute_intermediate_values() + + def _reset_target_pose(self, env_ids): + # reset goal rotation + rand_floats = sample_uniform(-1.0, 1.0, (len(env_ids), 2), device=self.device) + new_rot = randomize_rotation( + rand_floats[:, 0], rand_floats[:, 1], self.x_unit_tensor[env_ids], self.y_unit_tensor[env_ids] + ) + + # update goal pose and markers + self.goal_rot[env_ids] = new_rot + goal_pos = self.goal_pos + self.scene.env_origins + self.goal_markers.visualize(goal_pos, self.goal_rot) + + def _compute_intermediate_values(self): + # data for right hand + self.right_fingertip_pos = self.right_hand.data.body_pos_w[:, self.finger_bodies] + self.right_fingertip_rot = self.right_hand.data.body_quat_w[:, self.finger_bodies] + self.right_fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( + self.num_envs, self.num_fingertips, 3 + ) + self.right_fingertip_velocities = self.right_hand.data.body_vel_w[:, self.finger_bodies] + + self.right_hand_dof_pos = self.right_hand.data.joint_pos + self.right_hand_dof_vel = self.right_hand.data.joint_vel + + # data for left hand + self.left_fingertip_pos = self.left_hand.data.body_pos_w[:, self.finger_bodies] + self.left_fingertip_rot = self.left_hand.data.body_quat_w[:, self.finger_bodies] + self.left_fingertip_pos -= self.scene.env_origins.repeat((1, self.num_fingertips)).reshape( + self.num_envs, self.num_fingertips, 3 + ) + self.left_fingertip_velocities = self.left_hand.data.body_vel_w[:, self.finger_bodies] + + self.left_hand_dof_pos = self.left_hand.data.joint_pos + self.left_hand_dof_vel = self.left_hand.data.joint_vel + + # data for object + self.object_pos = self.object.data.root_pos_w - self.scene.env_origins + self.object_rot = self.object.data.root_quat_w + self.object_velocities = self.object.data.root_vel_w + self.object_linvel = self.object.data.root_lin_vel_w + self.object_angvel = self.object.data.root_ang_vel_w + + +@torch.jit.script +def scale(x, lower, upper): + return 0.5 * (x + 1.0) * (upper - lower) + lower + + +@torch.jit.script +def unscale(x, lower, upper): + return (2.0 * x - upper - lower) / (upper - lower) + + +@torch.jit.script +def randomize_rotation(rand0, rand1, x_unit_tensor, y_unit_tensor): + return quat_mul( + quat_from_angle_axis(rand0 * np.pi, x_unit_tensor), quat_from_angle_axis(rand1 * np.pi, y_unit_tensor) + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py new file mode 100644 index 00000000000..21e80b37dba --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py @@ -0,0 +1,231 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG + +import isaaclab.envs.mdp as mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.envs import DirectMARLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass + + +@configclass +class EventCfg: + """Configuration for randomization.""" + + # -- robot + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="reset", + min_step_count_between_reset=720, + params={ + "asset_cfg": SceneEntityCfg("right_hand"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (1.0, 1.0), + "restitution_range": (1.0, 1.0), + "num_buckets": 250, + }, + ) + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("right_hand", joint_names=".*"), + "stiffness_distribution_params": (0.75, 1.5), + "damping_distribution_params": (0.3, 3.0), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + robot_joint_pos_limits = EventTerm( + func=mdp.randomize_joint_parameters, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("right_hand", joint_names=".*"), + "lower_limit_distribution_params": (0.00, 0.01), + "upper_limit_distribution_params": (0.00, 0.01), + "operation": "add", + "distribution": "gaussian", + }, + ) + robot_tendon_properties = EventTerm( + func=mdp.randomize_fixed_tendon_parameters, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("right_hand", fixed_tendon_names=".*"), + "stiffness_distribution_params": (0.75, 1.5), + "damping_distribution_params": (0.3, 3.0), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + # -- object + object_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("object"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (1.0, 1.0), + "restitution_range": (1.0, 1.0), + "num_buckets": 250, + }, + ) + object_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + min_step_count_between_reset=720, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("object"), + "mass_distribution_params": (0.5, 1.5), + "operation": "scale", + "distribution": "uniform", + }, + ) + + # -- scene + reset_gravity = EventTerm( + func=mdp.randomize_physics_scene_gravity, + mode="interval", + is_global_time=True, + interval_range_s=(36.0, 36.0), # time_s = num_steps * (decimation * dt) + params={ + "gravity_distribution_params": ([0.0, 0.0, 0.0], [0.0, 0.0, 0.4]), + "operation": "add", + "distribution": "gaussian", + }, + ) + + +@configclass +class ShadowHandOverEnvCfg(DirectMARLEnvCfg): + # env + decimation = 2 + episode_length_s = 7.5 + possible_agents = ["right_hand", "left_hand"] + action_spaces = {"right_hand": 20, "left_hand": 20} + observation_spaces = {"right_hand": 157, "left_hand": 157} + state_space = 290 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 120, + render_interval=decimation, + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + physx=PhysxCfg( + bounce_threshold_velocity=0.2, + ), + ) + # robot + right_robot_cfg: ArticulationCfg = SHADOW_HAND_CFG.replace(prim_path="/World/envs/env_.*/RightRobot").replace( + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={".*": 0.0}, + ) + ) + left_robot_cfg: ArticulationCfg = SHADOW_HAND_CFG.replace(prim_path="/World/envs/env_.*/LeftRobot").replace( + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, -1.0, 0.5), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={".*": 0.0}, + ) + ) + actuated_joint_names = [ + "robot0_WRJ1", + "robot0_WRJ0", + "robot0_FFJ3", + "robot0_FFJ2", + "robot0_FFJ1", + "robot0_MFJ3", + "robot0_MFJ2", + "robot0_MFJ1", + "robot0_RFJ3", + "robot0_RFJ2", + "robot0_RFJ1", + "robot0_LFJ4", + "robot0_LFJ3", + "robot0_LFJ2", + "robot0_LFJ1", + "robot0_THJ4", + "robot0_THJ3", + "robot0_THJ2", + "robot0_THJ1", + "robot0_THJ0", + ] + fingertip_body_names = [ + "robot0_ffdistal", + "robot0_mfdistal", + "robot0_rfdistal", + "robot0_lfdistal", + "robot0_thdistal", + ] + + # in-hand object + object_cfg: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.SphereCfg( + radius=0.0335, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), + physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.7), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + collision_props=sim_utils.CollisionPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(density=500.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.54), rot=(1.0, 0.0, 0.0, 0.0)), + ) + # goal object + goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.SphereCfg( + radius=0.0335, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.4, 0.3, 1.0)), + ), + }, + ) + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=2048, env_spacing=1.5, replicate_physics=True) + + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # scales and constants + fall_dist = 0.24 + vel_obs_scale = 0.2 + act_moving_average = 1.0 + # reward-related scales + dist_reward_scale = 20.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/__init__.py new file mode 100644 index 00000000000..77b60a0b876 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Config-based workflow environments. +""" + +import gymnasium as gym diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/__init__.py new file mode 100644 index 00000000000..e243d3ee025 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Classic environments for control. + +These environments are based on the MuJoCo environments provided by OpenAI. + +Reference: + https://github.com/openai/gym/tree/master/gym/envs/mujoco +""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/__init__.py new file mode 100644 index 00000000000..ac7432a7d8c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Ant locomotion environment (similar to OpenAI Gym Ant-v2). +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Ant-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ant_env_cfg:AntEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AntPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..b391efc6d99 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: ant + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 32768 + mini_epochs: 4 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..4ac28aad94d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 32 + max_iterations = 1000 + save_interval = 50 + experiment_name = "ant" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[400, 200, 100], + critic_hidden_dims=[400, 200, 100], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml new file mode 100644 index 00000000000..2c2ed974aa4 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,24 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L161 +seed: 42 + +n_timesteps: !!float 1e7 +policy: 'MlpPolicy' +batch_size: 128 +n_steps: 512 +gamma: 0.99 +gae_lambda: 0.9 +n_epochs: 20 +ent_coef: 0.0 +sde_sample_freq: 4 +max_grad_norm: 0.5 +vf_coef: 0.5 +learning_rate: !!float 3e-5 +use_sde: True +clip_range: 0.4 +device: "cuda:0" +policy_kwargs: "dict( + log_std_init=-1, + ortho_init=False, + activation_fn=nn.ReLU, + net_arch=dict(pi=[256, 256], vf=[256, 256]) + )" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..945900eb128 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 4 + mini_batches: 2 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "ant" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 8000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py new file mode 100644 index 00000000000..a17802ae9cd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/ant/ant_env_cfg.py @@ -0,0 +1,189 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +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.terrains import TerrainImporterCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.classic.humanoid.mdp as mdp + +## +# Pre-defined configs +## +from isaaclab_assets.robots.ant import ANT_CFG # isort: skip + + +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with an ant robot.""" + + # terrain + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="average", + restitution_combine_mode="average", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # robot + robot = ANT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_effort = mdp.JointEffortActionCfg(asset_name="robot", joint_names=[".*"], scale=7.5) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for the policy.""" + + base_height = ObsTerm(func=mdp.base_pos_z) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel) + base_yaw_roll = ObsTerm(func=mdp.base_yaw_roll) + base_angle_to_target = ObsTerm(func=mdp.base_angle_to_target, params={"target_pos": (1000.0, 0.0, 0.0)}) + base_up_proj = ObsTerm(func=mdp.base_up_proj) + base_heading_proj = ObsTerm(func=mdp.base_heading_proj, params={"target_pos": (1000.0, 0.0, 0.0)}) + joint_pos_norm = ObsTerm(func=mdp.joint_pos_limit_normalized) + joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel, scale=0.2) + feet_body_forces = ObsTerm( + func=mdp.body_incoming_wrench, + scale=0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", body_names=["front_left_foot", "front_right_foot", "left_back_foot", "right_back_foot"] + ) + }, + ) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={"pose_range": {}, "velocity_range": {}}, + ) + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "position_range": (-0.2, 0.2), + "velocity_range": (-0.1, 0.1), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # (1) Reward for moving forward + progress = RewTerm(func=mdp.progress_reward, weight=1.0, params={"target_pos": (1000.0, 0.0, 0.0)}) + # (2) Stay alive bonus + alive = RewTerm(func=mdp.is_alive, weight=0.5) + # (3) Reward for non-upright posture + upright = RewTerm(func=mdp.upright_posture_bonus, weight=0.1, params={"threshold": 0.93}) + # (4) Reward for moving in the right direction + move_to_target = RewTerm( + func=mdp.move_to_target_bonus, weight=0.5, params={"threshold": 0.8, "target_pos": (1000.0, 0.0, 0.0)} + ) + # (5) Penalty for large action commands + action_l2 = RewTerm(func=mdp.action_l2, weight=-0.005) + # (6) Penalty for energy consumption + energy = RewTerm(func=mdp.power_consumption, weight=-0.05, params={"gear_ratio": {".*": 15.0}}) + # (7) Penalty for reaching close to joint limits + joint_pos_limits = RewTerm( + func=mdp.joint_pos_limits_penalty_ratio, weight=-0.1, params={"threshold": 0.99, "gear_ratio": {".*": 15.0}} + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Terminate if the episode length is exceeded + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) Terminate if the robot falls + torso_height = DoneTerm(func=mdp.root_height_below_minimum, params={"minimum_height": 0.31}) + + +@configclass +class AntEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the MuJoCo-style Ant walking environment.""" + + # Scene settings + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.episode_length_s = 16.0 + # simulation settings + self.sim.dt = 1 / 120.0 + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + # default friction material + self.sim.physics_material.static_friction = 1.0 + self.sim.physics_material.dynamic_friction = 1.0 + self.sim.physics_material.restitution = 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/__init__.py new file mode 100644 index 00000000000..8213aa1fa2b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/__init__.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Cartpole balancing environment. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Cartpole-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_env_cfg:CartpoleEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CartpolePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-RGB-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:CartpoleRGBCameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_camera_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-Depth-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:CartpoleDepthCameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_camera_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-RGB-ResNet18-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:CartpoleResNet18CameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_feature_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Cartpole-RGB-TheiaTiny-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.cartpole_camera_env_cfg:CartpoleTheiaTinyCameraEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_feature_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml new file mode 100644 index 00000000000..7d1dcf7945e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_camera_ppo_cfg.yaml @@ -0,0 +1,95 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + cnn: + type: conv2d + activation: relu + initializer: + name: default + regularizer: + name: None + convs: + - filters: 32 + kernel_size: 8 + strides: 4 + padding: 0 + - filters: 64 + kernel_size: 4 + strides: 2 + padding: 0 + - filters: 64 + kernel_size: 3 + strides: 1 + padding: 0 + + mlp: + units: [512] + activation: elu + initializer: + name: default + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole_camera + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: False + normalize_value: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 500 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 64 + minibatch_size: 2048 + mini_epochs: 4 + critic_coef: 2 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml new file mode 100644 index 00000000000..41e265e9f29 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_feature_ppo_cfg.yaml @@ -0,0 +1,79 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole_features + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstraop: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau : 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 200 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 2048 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..7e5707b194f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + # added to the wrapper + clip_observations: 5.0 + # can make custom wrapper? + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + # doesn't have this fine grained control but made it close + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [32, 32] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: cartpole + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: False + normalize_value: False + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1.0 + normalize_advantage: False + gamma: 0.99 + tau : 0.95 + learning_rate: 3e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 20000 + max_epochs: 150 + save_best_after: 50 + save_frequency: 25 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 16 + minibatch_size: 8192 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..b3186243fd2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 150 + save_interval = 50 + experiment_name = "cartpole" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[32, 32], + critic_hidden_dims=[32, 32], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml new file mode 100644 index 00000000000..5856f35f8e8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,21 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 42 + +n_timesteps: !!float 1e6 +policy: 'MlpPolicy' +n_steps: 16 +batch_size: 4096 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 20 +ent_coef: 0.01 +learning_rate: !!float 3e-4 +clip_range: !!float 0.2 +policy_kwargs: "dict( + activation_fn=nn.ELU, + net_arch=[32, 32], + squash_output=False, + )" +vf_coef: 1.0 +max_grad_norm: 1.0 +device: "cuda:0" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..d6f4edb6d53 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [32, 32] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 16 + learning_epochs: 8 + mini_batches: 8 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 3.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cartpole" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 2400 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py new file mode 100644 index 00000000000..8a09de731c5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_camera_env_cfg.py @@ -0,0 +1,181 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import TiledCameraCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.classic.cartpole.mdp as mdp + +from .cartpole_env_cfg import CartpoleEnvCfg, CartpoleSceneCfg + +## +# Scene definition +## + + +@configclass +class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): + + # add camera to the scene + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=100, + height=100, + ) + + +@configclass +class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): + + # add camera to the scene + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), + data_types=["distance_to_camera"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=100, + height=100, + ) + + +## +# MDP settings +## + + +@configclass +class RGBObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class RGBCameraPolicyCfg(ObsGroup): + """Observations for policy group with RGB images.""" + + image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb"}) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + policy: ObsGroup = RGBCameraPolicyCfg() + + +@configclass +class DepthObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class DepthCameraPolicyCfg(ObsGroup): + """Observations for policy group with depth images.""" + + image = ObsTerm( + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "distance_to_camera"} + ) + + policy: ObsGroup = DepthCameraPolicyCfg() + + +@configclass +class ResNet18ObservationCfg: + """Observation specifications for the MDP.""" + + @configclass + class ResNet18FeaturesCameraPolicyCfg(ObsGroup): + """Observations for policy group with features extracted from RGB images with a frozen ResNet18.""" + + image = ObsTerm( + func=mdp.image_features, + params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb", "model_name": "resnet18"}, + ) + + policy: ObsGroup = ResNet18FeaturesCameraPolicyCfg() + + +@configclass +class TheiaTinyObservationCfg: + """Observation specifications for the MDP.""" + + @configclass + class TheiaTinyFeaturesCameraPolicyCfg(ObsGroup): + """Observations for policy group with features extracted from RGB images with a frozen Theia-Tiny Transformer""" + + image = ObsTerm( + func=mdp.image_features, + params={ + "sensor_cfg": SceneEntityCfg("tiled_camera"), + "data_type": "rgb", + "model_name": "theia-tiny-patch16-224-cddsv", + "model_device": "cuda:0", + }, + ) + + policy: ObsGroup = TheiaTinyFeaturesCameraPolicyCfg() + + +## +# Environment configuration +## + + +@configclass +class CartpoleRGBCameraEnvCfg(CartpoleEnvCfg): + """Configuration for the cartpole environment with RGB camera.""" + + scene: CartpoleRGBCameraSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=512, env_spacing=20) + observations: RGBObservationsCfg = RGBObservationsCfg() + + def __post_init__(self): + super().__post_init__() + # remove ground as it obstructs the camera + self.scene.ground = None + # viewer settings + self.viewer.eye = (7.0, 0.0, 2.5) + self.viewer.lookat = (0.0, 0.0, 2.5) + + +@configclass +class CartpoleDepthCameraEnvCfg(CartpoleEnvCfg): + """Configuration for the cartpole environment with depth camera.""" + + scene: CartpoleDepthCameraSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=512, env_spacing=20) + observations: DepthObservationsCfg = DepthObservationsCfg() + + def __post_init__(self): + super().__post_init__() + # remove ground as it obstructs the camera + self.scene.ground = None + # viewer settings + self.viewer.eye = (7.0, 0.0, 2.5) + self.viewer.lookat = (0.0, 0.0, 2.5) + + +@configclass +class CartpoleResNet18CameraEnvCfg(CartpoleRGBCameraEnvCfg): + """Configuration for the cartpole environment with ResNet18 features as observations.""" + + observations: ResNet18ObservationCfg = ResNet18ObservationCfg() + + +@configclass +class CartpoleTheiaTinyCameraEnvCfg(CartpoleRGBCameraEnvCfg): + """Configuration for the cartpole environment with Theia-Tiny features as observations.""" + + observations: TheiaTinyObservationCfg = TheiaTinyObservationCfg() diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py new file mode 100644 index 00000000000..26d2d7cefe7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/cartpole_env_cfg.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +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.utils import configclass + +import isaaclab_tasks.manager_based.classic.cartpole.mdp as mdp + +## +# Pre-defined configs +## +from isaaclab_assets.robots.cartpole import CARTPOLE_CFG # isort:skip + + +## +# Scene definition +## + + +@configclass +class CartpoleSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)), + ) + + # cartpole + robot: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/DomeLight", + spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_effort = mdp.JointEffortActionCfg(asset_name="robot", joint_names=["slider_to_cart"], scale=100.0) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos_rel = ObsTerm(func=mdp.joint_pos_rel) + joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel) + + def __post_init__(self) -> None: + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # reset + reset_cart_position = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]), + "position_range": (-1.0, 1.0), + "velocity_range": (-0.5, 0.5), + }, + ) + + reset_pole_position = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"]), + "position_range": (-0.25 * math.pi, 0.25 * math.pi), + "velocity_range": (-0.25 * math.pi, 0.25 * math.pi), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # (1) Constant running reward + alive = RewTerm(func=mdp.is_alive, weight=1.0) + # (2) Failure penalty + terminating = RewTerm(func=mdp.is_terminated, weight=-2.0) + # (3) Primary task: keep pole upright + pole_pos = RewTerm( + func=mdp.joint_pos_target_l2, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"]), "target": 0.0}, + ) + # (4) Shaping tasks: lower cart velocity + cart_vel = RewTerm( + func=mdp.joint_vel_l1, + weight=-0.01, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"])}, + ) + # (5) Shaping tasks: lower pole angular velocity + pole_vel = RewTerm( + func=mdp.joint_vel_l1, + weight=-0.005, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"])}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Time out + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) Cart out of bounds + cart_out_of_bounds = DoneTerm( + func=mdp.joint_pos_out_of_manual_limit, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]), "bounds": (-3.0, 3.0)}, + ) + + +## +# Environment configuration +## + + +@configclass +class CartpoleEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the cartpole environment.""" + + # Scene settings + scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + events: EventCfg = EventCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + # Post initialization + def __post_init__(self) -> None: + """Post initialization.""" + # general settings + self.decimation = 2 + self.episode_length_s = 5 + # viewer settings + self.viewer.eye = (8.0, 0.0, 5.0) + # simulation settings + self.sim.dt = 1 / 120 + self.sim.render_interval = self.decimation diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py new file mode 100644 index 00000000000..0772cd92c24 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the cartpole environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .rewards import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py new file mode 100644 index 00000000000..690779ed441 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/cartpole/mdp/rewards.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import wrap_to_pi + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def joint_pos_target_l2(env: ManagerBasedRLEnv, target: float, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize joint position deviation from a target value.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # wrap the joint positions to (-pi, pi) + joint_pos = wrap_to_pi(asset.data.joint_pos[:, asset_cfg.joint_ids]) + # compute the reward + return torch.sum(torch.square(joint_pos - target), dim=1) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/__init__.py new file mode 100644 index 00000000000..7ca47217b42 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Humanoid locomotion environment (similar to OpenAI Gym Humanoid-v2). +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Humanoid-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.humanoid_env_cfg:HumanoidEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..95e5f8c4b3b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,75 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [400, 200, 100] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: humanoid + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.01 + score_to_win: 20000 + max_epochs: 1000 + save_best_after: 100 + save_frequency: 100 + grad_norm: 1.0 + entropy_coef: 0.0 + truncate_grads: True + e_clip: 0.2 + horizon_length: 32 + minibatch_size: 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..adc3ecae6d9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 32 + max_iterations = 1000 + save_interval = 50 + experiment_name = "humanoid" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[400, 200, 100], + critic_hidden_dims=[400, 200, 100], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml new file mode 100644 index 00000000000..b6db545ff22 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,22 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L245 +seed: 42 + +policy: 'MlpPolicy' +n_timesteps: !!float 5e7 +batch_size: 256 +n_steps: 512 +gamma: 0.99 +learning_rate: !!float 2.5e-4 +ent_coef: 0.0 +clip_range: 0.2 +n_epochs: 10 +gae_lambda: 0.95 +max_grad_norm: 1.0 +vf_coef: 0.5 +device: "cuda:0" +policy_kwargs: "dict( + log_std_init=-1, + ortho_init=False, + activation_fn=nn.ReLU, + net_arch=dict(pi=[256, 256], vf=[256, 256]) + )" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..9c668ca8315 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [400, 200, 100] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [400, 200, 100] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 32 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "humanoid" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 32000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py new file mode 100644 index 00000000000..d458bdcb26c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/humanoid_env_cfg.py @@ -0,0 +1,274 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +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.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +import isaaclab_tasks.manager_based.classic.humanoid.mdp as mdp + +## +# Scene definition +## + + +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with a humanoid robot.""" + + # terrain + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=1.0, dynamic_friction=1.0, restitution=0.0), + debug_vis=False, + ) + + # robot + robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=None, + max_depenetration_velocity=10.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.34), + joint_pos={".*": 0.0}, + ), + actuators={ + "body": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness={ + ".*_waist.*": 20.0, + ".*_upper_arm.*": 10.0, + "pelvis": 10.0, + ".*_lower_arm": 2.0, + ".*_thigh:0": 10.0, + ".*_thigh:1": 20.0, + ".*_thigh:2": 10.0, + ".*_shin": 5.0, + ".*_foot.*": 2.0, + }, + damping={ + ".*_waist.*": 5.0, + ".*_upper_arm.*": 5.0, + "pelvis": 5.0, + ".*_lower_arm": 1.0, + ".*_thigh:0": 5.0, + ".*_thigh:1": 5.0, + ".*_thigh:2": 5.0, + ".*_shin": 0.1, + ".*_foot.*": 1.0, + }, + ), + }, + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_effort = mdp.JointEffortActionCfg( + asset_name="robot", + joint_names=[".*"], + scale={ + ".*_waist.*": 67.5, + ".*_upper_arm.*": 67.5, + "pelvis": 67.5, + ".*_lower_arm": 45.0, + ".*_thigh:0": 45.0, + ".*_thigh:1": 135.0, + ".*_thigh:2": 45.0, + ".*_shin": 90.0, + ".*_foot.*": 22.5, + }, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for the policy.""" + + base_height = ObsTerm(func=mdp.base_pos_z) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, scale=0.25) + base_yaw_roll = ObsTerm(func=mdp.base_yaw_roll) + base_angle_to_target = ObsTerm(func=mdp.base_angle_to_target, params={"target_pos": (1000.0, 0.0, 0.0)}) + base_up_proj = ObsTerm(func=mdp.base_up_proj) + base_heading_proj = ObsTerm(func=mdp.base_heading_proj, params={"target_pos": (1000.0, 0.0, 0.0)}) + joint_pos_norm = ObsTerm(func=mdp.joint_pos_limit_normalized) + joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel, scale=0.1) + feet_body_forces = ObsTerm( + func=mdp.body_incoming_wrench, + scale=0.01, + params={"asset_cfg": SceneEntityCfg("robot", body_names=["left_foot", "right_foot"])}, + ) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={"pose_range": {}, "velocity_range": {}}, + ) + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "position_range": (-0.2, 0.2), + "velocity_range": (-0.1, 0.1), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # (1) Reward for moving forward + progress = RewTerm(func=mdp.progress_reward, weight=1.0, params={"target_pos": (1000.0, 0.0, 0.0)}) + # (2) Stay alive bonus + alive = RewTerm(func=mdp.is_alive, weight=2.0) + # (3) Reward for non-upright posture + upright = RewTerm(func=mdp.upright_posture_bonus, weight=0.1, params={"threshold": 0.93}) + # (4) Reward for moving in the right direction + move_to_target = RewTerm( + func=mdp.move_to_target_bonus, weight=0.5, params={"threshold": 0.8, "target_pos": (1000.0, 0.0, 0.0)} + ) + # (5) Penalty for large action commands + action_l2 = RewTerm(func=mdp.action_l2, weight=-0.01) + # (6) Penalty for energy consumption + energy = RewTerm( + func=mdp.power_consumption, + weight=-0.005, + params={ + "gear_ratio": { + ".*_waist.*": 67.5, + ".*_upper_arm.*": 67.5, + "pelvis": 67.5, + ".*_lower_arm": 45.0, + ".*_thigh:0": 45.0, + ".*_thigh:1": 135.0, + ".*_thigh:2": 45.0, + ".*_shin": 90.0, + ".*_foot.*": 22.5, + } + }, + ) + # (7) Penalty for reaching close to joint limits + joint_pos_limits = RewTerm( + func=mdp.joint_pos_limits_penalty_ratio, + weight=-0.25, + params={ + "threshold": 0.98, + "gear_ratio": { + ".*_waist.*": 67.5, + ".*_upper_arm.*": 67.5, + "pelvis": 67.5, + ".*_lower_arm": 45.0, + ".*_thigh:0": 45.0, + ".*_thigh:1": 135.0, + ".*_thigh:2": 45.0, + ".*_shin": 90.0, + ".*_foot.*": 22.5, + }, + }, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Terminate if the episode length is exceeded + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) Terminate if the robot falls + torso_height = DoneTerm(func=mdp.root_height_below_minimum, params={"minimum_height": 0.8}) + + +@configclass +class HumanoidEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the MuJoCo-style Humanoid walking environment.""" + + # Scene settings + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=5.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.episode_length_s = 16.0 + # simulation settings + self.sim.dt = 1 / 120.0 + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + # default friction material + self.sim.physics_material.static_friction = 1.0 + self.sim.physics_material.dynamic_friction = 1.0 + self.sim.physics_material.restitution = 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py new file mode 100644 index 00000000000..95410bd7e61 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the humanoid environment.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * +from .rewards import * diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py new file mode 100644 index 00000000000..9555d26b86a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/observations.py @@ -0,0 +1,80 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def base_yaw_roll(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Yaw and roll of the base in the simulation world frame.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # extract euler angles (in world frame) + roll, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + # normalize angle to [-pi, pi] + roll = torch.atan2(torch.sin(roll), torch.cos(roll)) + yaw = torch.atan2(torch.sin(yaw), torch.cos(yaw)) + + return torch.cat((yaw.unsqueeze(-1), roll.unsqueeze(-1)), dim=-1) + + +def base_up_proj(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Projection of the base up vector onto the world up vector.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute base up vector + base_up_vec = -asset.data.projected_gravity_b + + return base_up_vec[:, 2].unsqueeze(-1) + + +def base_heading_proj( + env: ManagerBasedEnv, target_pos: tuple[float, float, float], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Projection of the base forward vector onto the world forward vector.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute desired heading direction + to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_pos_w[:, :3] + to_target_pos[:, 2] = 0.0 + to_target_dir = math_utils.normalize(to_target_pos) + # compute base forward vector + heading_vec = math_utils.quat_apply(asset.data.root_quat_w, asset.data.FORWARD_VEC_B) + # compute dot product between heading and target direction + heading_proj = torch.bmm(heading_vec.view(env.num_envs, 1, 3), to_target_dir.view(env.num_envs, 3, 1)) + + return heading_proj.view(env.num_envs, 1) + + +def base_angle_to_target( + env: ManagerBasedEnv, target_pos: tuple[float, float, float], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Angle between the base forward vector and the vector to the target.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute desired heading direction + to_target_pos = torch.tensor(target_pos, device=env.device) - asset.data.root_pos_w[:, :3] + walk_target_angle = torch.atan2(to_target_pos[:, 1], to_target_pos[:, 0]) + # compute base forward vector + _, _, yaw = math_utils.euler_xyz_from_quat(asset.data.root_quat_w) + # normalize angle to target to [-pi, pi] + angle_to_target = walk_target_angle - yaw + angle_to_target = torch.atan2(torch.sin(angle_to_target), torch.cos(angle_to_target)) + + return angle_to_target.unsqueeze(-1) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py new file mode 100644 index 00000000000..53e97468820 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/classic/humanoid/mdp/rewards.py @@ -0,0 +1,148 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +import isaaclab.utils.string as string_utils +from isaaclab.assets import Articulation +from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg + +from . import observations as obs + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def upright_posture_bonus( + env: ManagerBasedRLEnv, threshold: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward for maintaining an upright posture.""" + up_proj = obs.base_up_proj(env, asset_cfg).squeeze(-1) + return (up_proj > threshold).float() + + +def move_to_target_bonus( + env: ManagerBasedRLEnv, + threshold: float, + target_pos: tuple[float, float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Reward for moving to the target heading.""" + heading_proj = obs.base_heading_proj(env, target_pos, asset_cfg).squeeze(-1) + return torch.where(heading_proj > threshold, 1.0, heading_proj / threshold) + + +class progress_reward(ManagerTermBase): + """Reward for making progress towards the target.""" + + def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): + # initialize the base class + super().__init__(cfg, env) + # create history buffer + self.potentials = torch.zeros(env.num_envs, device=env.device) + self.prev_potentials = torch.zeros_like(self.potentials) + + def reset(self, env_ids: torch.Tensor): + # extract the used quantities (to enable type-hinting) + asset: Articulation = self._env.scene["robot"] + # compute projection of current heading to desired heading vector + target_pos = torch.tensor(self.cfg.params["target_pos"], device=self.device) + to_target_pos = target_pos - asset.data.root_pos_w[env_ids, :3] + # reward terms + self.potentials[env_ids] = -torch.norm(to_target_pos, p=2, dim=-1) / self._env.step_dt + self.prev_potentials[env_ids] = self.potentials[env_ids] + + def __call__( + self, + env: ManagerBasedRLEnv, + target_pos: tuple[float, float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + ) -> torch.Tensor: + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute vector to target + target_pos = torch.tensor(target_pos, device=env.device) + to_target_pos = target_pos - asset.data.root_pos_w[:, :3] + to_target_pos[:, 2] = 0.0 + # update history buffer and compute new potential + self.prev_potentials[:] = self.potentials[:] + self.potentials[:] = -torch.norm(to_target_pos, p=2, dim=-1) / env.step_dt + + return self.potentials - self.prev_potentials + + +class joint_pos_limits_penalty_ratio(ManagerTermBase): + """Penalty for violating joint position limits weighted by the gear ratio.""" + + def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): + # add default argument + asset_cfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # resolve the gear ratio for each joint + self.gear_ratio = torch.ones(env.num_envs, asset.num_joints, device=env.device) + index_list, _, value_list = string_utils.resolve_matching_names_values( + cfg.params["gear_ratio"], asset.joint_names + ) + self.gear_ratio[:, index_list] = torch.tensor(value_list, device=env.device) + self.gear_ratio_scaled = self.gear_ratio / torch.max(self.gear_ratio) + + def __call__( + self, + env: ManagerBasedRLEnv, + threshold: float, + gear_ratio: dict[str, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + ) -> torch.Tensor: + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # compute the penalty over normalized joints + joint_pos_scaled = math_utils.scale_transform( + asset.data.joint_pos, asset.data.soft_joint_pos_limits[..., 0], asset.data.soft_joint_pos_limits[..., 1] + ) + # scale the violation amount by the gear ratio + violation_amount = (torch.abs(joint_pos_scaled) - threshold) / (1 - threshold) + violation_amount = violation_amount * self.gear_ratio_scaled + + return torch.sum((torch.abs(joint_pos_scaled) > threshold) * violation_amount, dim=-1) + + +class power_consumption(ManagerTermBase): + """Penalty for the power consumed by the actions to the environment. + + This is computed as commanded torque times the joint velocity. + """ + + def __init__(self, env: ManagerBasedRLEnv, cfg: RewardTermCfg): + # add default argument + asset_cfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + + # resolve the gear ratio for each joint + self.gear_ratio = torch.ones(env.num_envs, asset.num_joints, device=env.device) + index_list, _, value_list = string_utils.resolve_matching_names_values( + cfg.params["gear_ratio"], asset.joint_names + ) + self.gear_ratio[:, index_list] = torch.tensor(value_list, device=env.device) + self.gear_ratio_scaled = self.gear_ratio / torch.max(self.gear_ratio) + + def __call__( + self, env: ManagerBasedRLEnv, gear_ratio: dict[str, float], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") + ) -> torch.Tensor: + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # return power = torque * velocity (here actions: joint torques) + return torch.sum(torch.abs(env.action_manager.action * asset.data.joint_vel * self.gear_ratio_scaled), dim=-1) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/__init__.py new file mode 100644 index 00000000000..e9c9666648c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Locomotion environments for legged robots.""" + +from .velocity import * # noqa diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py new file mode 100644 index 00000000000..8f36fa3d9c1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Locomotion environments with velocity-tracking commands. + +These environments are based on the `legged_gym` environments provided by Rudin et al. + +Reference: + https://github.com/leggedrobotics/legged_gym +""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py new file mode 100644 index 00000000000..685ad983408 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for velocity-based locomotion environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py new file mode 100644 index 00000000000..0a2af10b15f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/__init__.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Unitree-A1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeA1FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeA1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Unitree-A1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeA1FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeA1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-A1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeA1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeA1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-A1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeA1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeA1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..6846e1cbcfc --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class UnitreeA1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "unitree_a1_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class UnitreeA1FlatPPORunnerCfg(UnitreeA1RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "unitree_a1_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..5e2a20e0764 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_a1_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..17dbaa0c8e6 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_a1_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py new file mode 100644 index 00000000000..c00d4ace036 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import UnitreeA1RoughEnvCfg + + +@configclass +class UnitreeA1FlatEnvCfg(UnitreeA1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 0.25 + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class UnitreeA1FlatEnvCfg_PLAY(UnitreeA1FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py new file mode 100644 index 00000000000..15e808d062a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.unitree import UNITREE_A1_CFG # isort: skip + + +@configclass +class UnitreeA1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.robot = UNITREE_A1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" + # scale down the terrains because the robot is small + self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01 + + # reduce action scale + self.actions.joint_pos.scale = 0.25 + + # event + self.events.push_robot = None + self.events.add_base_mass.params["mass_distribution_params"] = (-1.0, 3.0) + self.events.add_base_mass.params["asset_cfg"].body_names = "trunk" + self.events.base_external_force_torque.params["asset_cfg"].body_names = "trunk" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # rewards + self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" + self.rewards.feet_air_time.weight = 0.01 + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -0.0002 + self.rewards.track_lin_vel_xy_exp.weight = 1.5 + self.rewards.track_ang_vel_z_exp.weight = 0.75 + self.rewards.dof_acc_l2.weight = -2.5e-7 + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "trunk" + + +@configclass +class UnitreeA1RoughEnvCfg_PLAY(UnitreeA1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py new file mode 100644 index 00000000000..8ed3433320d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/__init__.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Anymal-B-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalBFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Anymal-B-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalBFlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-B-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalBRoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-B-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalBRoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalBRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..7704cdb9610 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AnymalBRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "anymal_b_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class AnymalBFlatPPORunnerCfg(AnymalBRoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "anymal_b_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..7715792ff6f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_b_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..480b038e0d7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_b_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py new file mode 100644 index 00000000000..62eeab14719 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import AnymalBRoughEnvCfg + + +@configclass +class AnymalBFlatEnvCfg(AnymalBRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -5.0 + self.rewards.dof_torques_l2.weight = -2.5e-5 + self.rewards.feet_air_time.weight = 0.5 + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class AnymalBFlatEnvCfg_PLAY(AnymalBFlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py new file mode 100644 index 00000000000..5951cedd327 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/rough_env_cfg.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets import ANYMAL_B_CFG # isort: skip + + +@configclass +class AnymalBRoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to anymal-d + self.scene.robot = ANYMAL_B_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class AnymalBRoughEnvCfg_PLAY(AnymalBRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py new file mode 100644 index 00000000000..c6e79395d0b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/__init__.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Anymal-C-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalCFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_flat_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Anymal-C-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalCFlatEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_flat_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-C-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalCRoughEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-C-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalCRoughEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalCRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..c472ce673cf --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_flat_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [128, 128, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: anymal_c_flat + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: False + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 20000 + max_epochs: 300 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.005 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2.0 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..042799da1ee --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rl_games_rough_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: anymal_c_rough + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: True + normalize_input: False + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.6 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 20000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 50 + grad_norm: 1.0 + entropy_coef: 0.005 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2.0 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..c0100f7998e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "anymal_c_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class AnymalCFlatPPORunnerCfg(AnymalCRoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "anymal_c_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..cb90016afa5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_c_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..d7c6faadeab --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_c_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py new file mode 100644 index 00000000000..193658ad33a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import AnymalCRoughEnvCfg + + +@configclass +class AnymalCFlatEnvCfg(AnymalCRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -5.0 + self.rewards.dof_torques_l2.weight = -2.5e-5 + self.rewards.feet_air_time.weight = 0.5 + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class AnymalCFlatEnvCfg_PLAY(AnymalCFlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py new file mode 100644 index 00000000000..2fd75f8ec57 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/rough_env_cfg.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip + + +@configclass +class AnymalCRoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to anymal-c + self.scene.robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class AnymalCRoughEnvCfg_PLAY(AnymalCRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py new file mode 100644 index 00000000000..188acdf8dd0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/__init__.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Anymal-D-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalDFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Anymal-D-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:AnymalDFlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-D-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalDRoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Anymal-D-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:AnymalDRoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AnymalDRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..75a6972c1da --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AnymalDRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "anymal_d_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class AnymalDFlatPPORunnerCfg(AnymalDRoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "anymal_d_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..b770447834b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_d_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..465c0c080dd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_d_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py new file mode 100644 index 00000000000..37e0e3ba13a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import AnymalDRoughEnvCfg + + +@configclass +class AnymalDFlatEnvCfg(AnymalDRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -5.0 + self.rewards.dof_torques_l2.weight = -2.5e-5 + self.rewards.feet_air_time.weight = 0.5 + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class AnymalDFlatEnvCfg_PLAY(AnymalDFlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py new file mode 100644 index 00000000000..b00eea79f93 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py @@ -0,0 +1,51 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.anymal import ANYMAL_D_CFG # isort: skip + + +@configclass +class AnymalDRoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to anymal-d + self.scene.robot = ANYMAL_D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class AnymalDRoughEnvCfg_PLAY(AnymalDRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py new file mode 100644 index 00000000000..ed0f41a96f8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/__init__.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Cassie-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:CassieFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CassieFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Cassie-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:CassieFlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CassieFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Cassie-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:CassieRoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CassieRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Cassie-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:CassieRoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CassieRoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..a83b604e64e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class CassieRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "cassie_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class CassieFlatPPORunnerCfg(CassieRoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 1000 + self.experiment_name = "cassie_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..2c7eb12c32d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cassie_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 24000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..0b5686f3971 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "cassie_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py new file mode 100644 index 00000000000..0a769c81d7a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/flat_env_cfg.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import CassieRoughEnvCfg + + +@configclass +class CassieFlatEnvCfg(CassieRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 5.0 + self.rewards.joint_deviation_hip.params["asset_cfg"].joint_names = ["hip_rotation_.*"] + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class CassieFlatEnvCfg_PLAY(CassieFlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py new file mode 100644 index 00000000000..7dfa302319c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg, RewardsCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.cassie import CASSIE_CFG # isort: skip + + +@configclass +class CassieRewardsCfg(RewardsCfg): + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=2.5, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*toe"), + "command_name": "base_velocity", + "threshold": 0.3, + }, + ) + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["hip_abduction_.*", "hip_rotation_.*"])}, + ) + joint_deviation_toes = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["toe_joint_.*"])}, + ) + # penalize toe joint limits + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="toe_joint_.*")}, + ) + + +@configclass +class CassieRoughEnvCfg(LocomotionVelocityRoughEnvCfg): + """Cassie rough environment configuration.""" + + rewards: CassieRewardsCfg = CassieRewardsCfg() + + def __post_init__(self): + super().__post_init__() + # scene + self.scene.robot = CASSIE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis" + + # actions + self.actions.joint_pos.scale = 0.5 + + # events + self.events.push_robot = None + self.events.add_base_mass = None + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.base_external_force_torque.params["asset_cfg"].body_names = [".*pelvis"] + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = [".*pelvis"] + + # rewards + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -5.0e-6 + self.rewards.track_lin_vel_xy_exp.weight = 2.0 + self.rewards.track_ang_vel_z_exp.weight = 1.0 + self.rewards.action_rate_l2.weight *= 1.5 + self.rewards.dof_acc_l2.weight *= 1.5 + + +@configclass +class CassieRoughEnvCfg_PLAY(CassieRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + self.commands.base_velocity.ranges.lin_vel_x = (0.7, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.heading = (0.0, 0.0) + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py new file mode 100644 index 00000000000..a8a6ce7fbe2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/__init__.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Rough-G1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Rough-G1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-G1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:G1FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-G1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:G1FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..7a5bffe16f7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 3000 + save_interval = 50 + experiment_name = "g1_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.008, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class G1FlatPPORunnerCfg(G1RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 1500 + self.experiment_name = "g1_flat" + self.policy.actor_hidden_dims = [256, 128, 128] + self.policy.critic_hidden_dims = [256, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..a25b969a912 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.008 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "g1_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..07e71559e52 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.008 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "g1_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 72000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py new file mode 100644 index 00000000000..70ad485bd5b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/flat_env_cfg.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from .rough_env_cfg import G1RoughEnvCfg + + +@configclass +class G1FlatEnvCfg(G1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + # Rewards + self.rewards.track_ang_vel_z_exp.weight = 1.0 + self.rewards.lin_vel_z_l2.weight = -0.2 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -1.0e-7 + self.rewards.feet_air_time.weight = 0.75 + self.rewards.feet_air_time.params["threshold"] = 0.4 + self.rewards.dof_torques_l2.weight = -2.0e-6 + self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] + ) + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + + +class G1FlatEnvCfg_PLAY(G1FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py new file mode 100644 index 00000000000..47633154d57 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py @@ -0,0 +1,185 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg, RewardsCfg + +## +# Pre-defined configs +## +from isaaclab_assets import G1_MINIMAL_CFG # isort: skip + + +@configclass +class G1Rewards(RewardsCfg): + """Reward terms for the MDP.""" + + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5} + ) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=0.25, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), + "threshold": 0.4, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.1, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_roll_link"), + }, + ) + + # Penalize ankle joint limits + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"])}, + ) + # Penalize deviation from default of the joints that are not essential for locomotion + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw_joint", ".*_hip_roll_joint"])}, + ) + joint_deviation_arms = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_pitch_joint", + ".*_elbow_roll_joint", + ], + ) + }, + ) + joint_deviation_fingers = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.05, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_five_joint", + ".*_three_joint", + ".*_six_joint", + ".*_four_joint", + ".*_zero_joint", + ".*_one_joint", + ".*_two_joint", + ], + ) + }, + ) + joint_deviation_torso = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="torso_joint")}, + ) + + +@configclass +class G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + rewards: G1Rewards = G1Rewards() + + def __post_init__(self): + # post init of parent + super().__post_init__() + # Scene + self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + + # Randomization + self.events.push_robot = None + self.events.add_base_mass = None + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.base_external_force_torque.params["asset_cfg"].body_names = ["torso_link"] + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # Rewards + self.rewards.lin_vel_z_l2.weight = 0.0 + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.0 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -1.25e-7 + self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] + ) + self.rewards.dof_torques_l2.weight = -1.5e-7 + self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint", ".*_ankle_.*"] + ) + + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (-0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" + + +@configclass +class G1RoughEnvCfg_PLAY(G1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + self.episode_length_s = 40.0 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + self.commands.base_velocity.ranges.heading = (0.0, 0.0) + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py new file mode 100644 index 00000000000..651a2ccf078 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/__init__.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeGo1FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeGo1FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeGo1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeGo1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..a0e8652342f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class UnitreeGo1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "unitree_go1_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class UnitreeGo1FlatPPORunnerCfg(UnitreeGo1RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "unitree_go1_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..dabee2d24bd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_go1_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..bd87c9b22b7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_go1_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py new file mode 100644 index 00000000000..741fcc528ab --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import UnitreeGo1RoughEnvCfg + + +@configclass +class UnitreeGo1FlatEnvCfg(UnitreeGo1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 0.25 + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class UnitreeGo1FlatEnvCfg_PLAY(UnitreeGo1FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py new file mode 100644 index 00000000000..db26bbe0e1e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.unitree import UNITREE_GO1_CFG # isort: skip + + +@configclass +class UnitreeGo1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.robot = UNITREE_GO1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" + # scale down the terrains because the robot is small + self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01 + + # reduce action scale + self.actions.joint_pos.scale = 0.25 + + # event + self.events.push_robot = None + self.events.add_base_mass.params["mass_distribution_params"] = (-1.0, 3.0) + self.events.add_base_mass.params["asset_cfg"].body_names = "trunk" + self.events.base_external_force_torque.params["asset_cfg"].body_names = "trunk" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # rewards + self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" + self.rewards.feet_air_time.weight = 0.01 + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -0.0002 + self.rewards.track_lin_vel_xy_exp.weight = 1.5 + self.rewards.track_ang_vel_z_exp.weight = 0.75 + self.rewards.dof_acc_l2.weight = -2.5e-7 + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "trunk" + + +@configclass +class UnitreeGo1RoughEnvCfg_PLAY(UnitreeGo1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py new file mode 100644 index 00000000000..20862eebf9c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/__init__.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeGo2FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Unitree-Go2-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:UnitreeGo2FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeGo2RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-Unitree-Go2-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:UnitreeGo2RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UnitreeGo2RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..827a971e23e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class UnitreeGo2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "unitree_go2_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class UnitreeGo2FlatPPORunnerCfg(UnitreeGo2RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 300 + self.experiment_name = "unitree_go2_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..fea8fcc70c2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_go2_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 7200 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..808974198ec --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "unitree_go2_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py new file mode 100644 index 00000000000..0b9fd2b9793 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/flat_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import UnitreeGo2RoughEnvCfg + + +@configclass +class UnitreeGo2FlatEnvCfg(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # override rewards + self.rewards.flat_orientation_l2.weight = -2.5 + self.rewards.feet_air_time.weight = 0.25 + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + + +class UnitreeGo2FlatEnvCfg_PLAY(UnitreeGo2FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py new file mode 100644 index 00000000000..ceb0c274d8d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.unitree import UNITREE_GO2_CFG # isort: skip + + +@configclass +class UnitreeGo2RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.robot = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" + # scale down the terrains because the robot is small + self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) + self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01 + + # reduce action scale + self.actions.joint_pos.scale = 0.25 + + # event + self.events.push_robot = None + self.events.add_base_mass.params["mass_distribution_params"] = (-1.0, 3.0) + self.events.add_base_mass.params["asset_cfg"].body_names = "base" + self.events.base_external_force_torque.params["asset_cfg"].body_names = "base" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # rewards + self.rewards.feet_air_time.params["sensor_cfg"].body_names = ".*_foot" + self.rewards.feet_air_time.weight = 0.01 + self.rewards.undesired_contacts = None + self.rewards.dof_torques_l2.weight = -0.0002 + self.rewards.track_lin_vel_xy_exp.weight = 1.5 + self.rewards.track_ang_vel_z_exp.weight = 0.75 + self.rewards.dof_acc_l2.weight = -2.5e-7 + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "base" + + +@configclass +class UnitreeGo2RoughEnvCfg_PLAY(UnitreeGo2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py new file mode 100644 index 00000000000..0e23dbf4076 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/__init__.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Rough-H1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:H1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Rough-H1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:H1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H1RoughPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-H1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:H1FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-H1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:H1FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H1FlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..e78d3bb7013 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class H1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 3000 + save_interval = 50 + experiment_name = "h1_rough" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class H1FlatPPORunnerCfg(H1RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 1000 + self.experiment_name = "h1_flat" + self.policy.actor_hidden_dims = [128, 128, 128] + self.policy.critic_hidden_dims = [128, 128, 128] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..c509d4ee3fa --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "h1_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 24000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 00000000000..a841751fd6c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.995 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "h1_rough" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 72000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py new file mode 100644 index 00000000000..0090b4ba64e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/flat_env_cfg.py @@ -0,0 +1,46 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import H1RoughEnvCfg + + +@configclass +class H1FlatEnvCfg(H1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + self.rewards.feet_air_time.weight = 1.0 + self.rewards.feet_air_time.params["threshold"] = 0.6 + + +class H1FlatEnvCfg_PLAY(H1FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py new file mode 100644 index 00000000000..909f6ea4e34 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py @@ -0,0 +1,149 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg, RewardsCfg + +## +# Pre-defined configs +## +from isaaclab_assets import H1_MINIMAL_CFG # isort: skip + + +@configclass +class H1Rewards(RewardsCfg): + """Reward terms for the MDP.""" + + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + lin_vel_z_l2 = None + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=1.0, params={"command_name": "base_velocity", "std": 0.5} + ) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=0.25, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*ankle_link"), + "threshold": 0.4, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.25, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*ankle_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*ankle_link"), + }, + ) + # Penalize ankle joint limits + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, weight=-1.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_ankle")} + ) + # Penalize deviation from default of the joints that are not essential for locomotion + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw", ".*_hip_roll"])}, + ) + joint_deviation_arms = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_shoulder_.*", ".*_elbow"])}, + ) + joint_deviation_torso = RewTerm( + func=mdp.joint_deviation_l1, weight=-0.1, params={"asset_cfg": SceneEntityCfg("robot", joint_names="torso")} + ) + + +@configclass +class H1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + rewards: H1Rewards = H1Rewards() + + def __post_init__(self): + # post init of parent + super().__post_init__() + # Scene + self.scene.robot = H1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + if self.scene.height_scanner: + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + + # Randomization + self.events.push_robot = None + self.events.add_base_mass = None + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.base_external_force_torque.params["asset_cfg"].body_names = [".*torso_link"] + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + + # Terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = [".*torso_link"] + + # Rewards + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.0 + self.rewards.dof_torques_l2.weight = 0.0 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -1.25e-7 + + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = ".*torso_link" + + +@configclass +class H1RoughEnvCfg_PLAY(H1RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + self.episode_length_s = 40.0 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + self.commands.base_velocity.ranges.heading = (0.0, 0.0) + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/README.md b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/README.md new file mode 100644 index 00000000000..eec0d443109 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/README.md @@ -0,0 +1,9 @@ +# Acknowledgment + +We would like to acknowledge [The AI Institute](https://theaiinstitute.com/)'s efforts in developing +the Spot RL environment from the specifications provided by Boston Dynamics. +The team at The AI Institute trained, verified, and deployed the resulting policy on the Spot hardware. +They demonstrated its capability and reliability out in the real world. + +The accompanying deployment code and access to Spot's low-level API is available with the [Spot RL +Researcher Kit](https://bostondynamics.com/reinforcement-learning-researcher-kit/). diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py new file mode 100644 index 00000000000..0682930aed0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/__init__.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Flat-Spot-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:SpotFlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SpotFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Velocity-Flat-Spot-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:SpotFlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SpotFlatPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..9890e07c881 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class SpotFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 20000 + save_interval = 50 + experiment_name = "spot_flat" + empirical_normalization = False + store_code_state = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=0.5, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.0025, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..e412959c368 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.0025 + value_loss_scale: 0.5 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "spot_flat" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 480000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py new file mode 100644 index 00000000000..5b6390c8e73 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/flat_env_cfg.py @@ -0,0 +1,384 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +import isaaclab.terrains as terrain_gen +from isaaclab.envs import ViewerCfg +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, SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.locomotion.velocity.config.spot.mdp as spot_mdp +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.spot import SPOT_CFG # isort: skip + + +COBBLESTONE_ROAD_CFG = terrain_gen.TerrainGeneratorCfg( + size=(8.0, 8.0), + border_width=20.0, + num_rows=9, + num_cols=21, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + difficulty_range=(0.0, 1.0), + use_cache=False, + sub_terrains={ + "flat": terrain_gen.MeshPlaneTerrainCfg(proportion=0.2), + "random_rough": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.2, noise_range=(0.02, 0.05), noise_step=0.02, border_width=0.25 + ), + }, +) + + +@configclass +class SpotActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.2, use_default_offset=True) + + +@configclass +class SpotCommandsCfg: + """Command specifications for the MDP.""" + + base_velocity = mdp.UniformVelocityCommandCfg( + asset_name="robot", + resampling_time_range=(10.0, 10.0), + rel_standing_envs=0.1, + rel_heading_envs=0.0, + heading_command=False, + debug_vis=True, + ranges=mdp.UniformVelocityCommandCfg.Ranges( + lin_vel_x=(-2.0, 3.0), lin_vel_y=(-1.5, 1.5), ang_vel_z=(-2.0, 2.0) + ), + ) + + +@configclass +class SpotObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # `` observation terms (order preserved) + base_lin_vel = ObsTerm( + func=mdp.base_lin_vel, params={"asset_cfg": SceneEntityCfg("robot")}, noise=Unoise(n_min=-0.1, n_max=0.1) + ) + base_ang_vel = ObsTerm( + func=mdp.base_ang_vel, params={"asset_cfg": SceneEntityCfg("robot")}, noise=Unoise(n_min=-0.1, n_max=0.1) + ) + projected_gravity = ObsTerm( + func=mdp.projected_gravity, + params={"asset_cfg": SceneEntityCfg("robot")}, + noise=Unoise(n_min=-0.05, n_max=0.05), + ) + velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("robot")}, noise=Unoise(n_min=-0.05, n_max=0.05) + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, params={"asset_cfg": SceneEntityCfg("robot")}, noise=Unoise(n_min=-0.5, n_max=0.5) + ) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class SpotEventCfg: + """Configuration for randomization.""" + + # startup + physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.3, 1.0), + "dynamic_friction_range": (0.3, 0.8), + "restitution_range": (0.0, 0.0), + "num_buckets": 64, + }, + ) + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="body"), + "mass_distribution_params": (-2.5, 2.5), + "operation": "add", + }, + ) + + # reset + base_external_force_torque = EventTerm( + func=mdp.apply_external_force_torque, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="body"), + "force_range": (0.0, 0.0), + "torque_range": (-0.0, 0.0), + }, + ) + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot"), + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (-1.5, 1.5), + "y": (-1.0, 1.0), + "z": (-0.5, 0.5), + "roll": (-0.7, 0.7), + "pitch": (-0.7, 0.7), + "yaw": (-1.0, 1.0), + }, + }, + ) + + reset_robot_joints = EventTerm( + func=spot_mdp.reset_joints_around_default, + mode="reset", + params={ + "position_range": (-0.2, 0.2), + "velocity_range": (-2.5, 2.5), + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + # interval + push_robot = EventTerm( + func=mdp.push_by_setting_velocity, + mode="interval", + interval_range_s=(10.0, 15.0), + params={ + "asset_cfg": SceneEntityCfg("robot"), + "velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}, + }, + ) + + +@configclass +class SpotRewardsCfg: + # -- task + air_time = RewardTermCfg( + func=spot_mdp.air_time_reward, + weight=5.0, + params={ + "mode_time": 0.3, + "velocity_threshold": 0.5, + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_foot"), + }, + ) + base_angular_velocity = RewardTermCfg( + func=spot_mdp.base_angular_velocity_reward, + weight=5.0, + params={"std": 2.0, "asset_cfg": SceneEntityCfg("robot")}, + ) + base_linear_velocity = RewardTermCfg( + func=spot_mdp.base_linear_velocity_reward, + weight=5.0, + params={"std": 1.0, "ramp_rate": 0.5, "ramp_at_vel": 1.0, "asset_cfg": SceneEntityCfg("robot")}, + ) + foot_clearance = RewardTermCfg( + func=spot_mdp.foot_clearance_reward, + weight=0.5, + params={ + "std": 0.05, + "tanh_mult": 2.0, + "target_height": 0.1, + "asset_cfg": SceneEntityCfg("robot", body_names=".*_foot"), + }, + ) + gait = RewardTermCfg( + func=spot_mdp.GaitReward, + weight=10.0, + params={ + "std": 0.1, + "max_err": 0.2, + "velocity_threshold": 0.5, + "synced_feet_pair_names": (("fl_foot", "hr_foot"), ("fr_foot", "hl_foot")), + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("contact_forces"), + }, + ) + + # -- penalties + action_smoothness = RewardTermCfg(func=spot_mdp.action_smoothness_penalty, weight=-1.0) + air_time_variance = RewardTermCfg( + func=spot_mdp.air_time_variance_penalty, + weight=-1.0, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_foot")}, + ) + base_motion = RewardTermCfg( + func=spot_mdp.base_motion_penalty, weight=-2.0, params={"asset_cfg": SceneEntityCfg("robot")} + ) + base_orientation = RewardTermCfg( + func=spot_mdp.base_orientation_penalty, weight=-3.0, params={"asset_cfg": SceneEntityCfg("robot")} + ) + foot_slip = RewardTermCfg( + func=spot_mdp.foot_slip_penalty, + weight=-0.5, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*_foot"), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_foot"), + "threshold": 1.0, + }, + ) + joint_acc = RewardTermCfg( + func=spot_mdp.joint_acceleration_penalty, + weight=-1.0e-4, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_h[xy]")}, + ) + joint_pos = RewardTermCfg( + func=spot_mdp.joint_position_penalty, + weight=-0.7, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "stand_still_scale": 5.0, + "velocity_threshold": 0.5, + }, + ) + joint_torques = RewardTermCfg( + func=spot_mdp.joint_torques_penalty, + weight=-5.0e-4, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}, + ) + joint_vel = RewardTermCfg( + func=spot_mdp.joint_velocity_penalty, + weight=-1.0e-2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_h[xy]")}, + ) + + +@configclass +class SpotTerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + body_contact = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["body", ".*leg"]), "threshold": 1.0}, + ) + terrain_out_of_bounds = DoneTerm( + func=mdp.terrain_out_of_bounds, + params={"asset_cfg": SceneEntityCfg("robot"), "distance_buffer": 3.0}, + time_out=True, + ) + + +@configclass +class SpotFlatEnvCfg(LocomotionVelocityRoughEnvCfg): + + # Basic settings' + observations: SpotObservationsCfg = SpotObservationsCfg() + actions: SpotActionsCfg = SpotActionsCfg() + commands: SpotCommandsCfg = SpotCommandsCfg() + + # MDP setting + rewards: SpotRewardsCfg = SpotRewardsCfg() + terminations: SpotTerminationsCfg = SpotTerminationsCfg() + events: SpotEventCfg = SpotEventCfg() + + # Viewer + viewer = ViewerCfg(eye=(10.5, 10.5, 0.3), origin_type="world", env_index=0, asset_name="robot") + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # general settings + self.decimation = 10 # 50 Hz + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 0.002 # 500 Hz + self.sim.render_interval = self.decimation + self.sim.physics_material.static_friction = 1.0 + self.sim.physics_material.dynamic_friction = 1.0 + self.sim.physics_material.friction_combine_mode = "multiply" + self.sim.physics_material.restitution_combine_mode = "multiply" + # update sensor update periods + # we tick all the sensors based on the smallest update period (physics update period) + self.scene.contact_forces.update_period = self.sim.dt + + # switch robot to Spot-d + self.scene.robot = SPOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # terrain + self.scene.terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=COBBLESTONE_ROAD_CFG, + max_init_terrain_level=COBBLESTONE_ROAD_CFG.num_rows - 1, + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + debug_vis=True, + ) + + # no height scan + self.scene.height_scanner = None + + +class SpotFlatEnvCfg_PLAY(SpotFlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + # self.events.base_external_force_torque = None + # self.events.push_robot = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py new file mode 100644 index 00000000000..8bde27da7ef --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""This sub-module contains the functions that are specific to the Spot locomotion task.""" + +from .events import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py new file mode 100644 index 00000000000..cfcffa5a7c5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/events.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""This sub-module contains the functions that can be used to enable Spot randomizations. + +The functions can be passed to the :class:`isaaclab.managers.EventTermCfg` object to enable +the randomization introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import sample_uniform + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def reset_joints_around_default( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: tuple[float, float], + velocity_range: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the robot joints in the interval around the default position and velocity by the given ranges. + + This function samples random values from the given ranges around the default joint positions and velocities. + The ranges are clipped to fit inside the soft joint limits. The sampled values are then set into the physics + simulation. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + # get default joint state + joint_min_pos = asset.data.default_joint_pos[env_ids] + position_range[0] + joint_max_pos = asset.data.default_joint_pos[env_ids] + position_range[1] + joint_min_vel = asset.data.default_joint_vel[env_ids] + velocity_range[0] + joint_max_vel = asset.data.default_joint_vel[env_ids] + velocity_range[1] + # clip pos to range + joint_pos_limits = asset.data.soft_joint_pos_limits[env_ids, ...] + joint_min_pos = torch.clamp(joint_min_pos, min=joint_pos_limits[..., 0], max=joint_pos_limits[..., 1]) + joint_max_pos = torch.clamp(joint_max_pos, min=joint_pos_limits[..., 0], max=joint_pos_limits[..., 1]) + # clip vel to range + joint_vel_abs_limits = asset.data.soft_joint_vel_limits[env_ids] + joint_min_vel = torch.clamp(joint_min_vel, min=-joint_vel_abs_limits, max=joint_vel_abs_limits) + joint_max_vel = torch.clamp(joint_max_vel, min=-joint_vel_abs_limits, max=joint_vel_abs_limits) + # sample these values randomly + joint_pos = sample_uniform(joint_min_pos, joint_max_pos, joint_min_pos.shape, joint_min_pos.device) + joint_vel = sample_uniform(joint_min_vel, joint_max_vel, joint_min_vel.shape, joint_min_vel.device) + # set into the physics simulation + asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py new file mode 100644 index 00000000000..b47c9ba8cc2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/mdp/rewards.py @@ -0,0 +1,287 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the reward functions that can be used for Spot's locomotion task. + +The functions can be passed to the :class:`isaaclab.managers.RewardTermCfg` object to +specify the reward function and its parameters. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.sensors import ContactSensor + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + from isaaclab.managers import RewardTermCfg + + +## +# Task Rewards +## + + +def air_time_reward( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + sensor_cfg: SceneEntityCfg, + mode_time: float, + velocity_threshold: float, +) -> torch.Tensor: + """Reward longer feet air and contact time.""" + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + asset: Articulation = env.scene[asset_cfg.name] + if contact_sensor.cfg.track_air_time is False: + raise RuntimeError("Activate ContactSensor's track_air_time!") + # compute the reward + current_air_time = contact_sensor.data.current_air_time[:, sensor_cfg.body_ids] + current_contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids] + + t_max = torch.max(current_air_time, current_contact_time) + t_min = torch.clip(t_max, max=mode_time) + stance_cmd_reward = torch.clip(current_contact_time - current_air_time, -mode_time, mode_time) + cmd = torch.norm(env.command_manager.get_command("base_velocity"), dim=1).unsqueeze(dim=1).expand(-1, 4) + body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1).unsqueeze(dim=1).expand(-1, 4) + reward = torch.where( + torch.logical_or(cmd > 0.0, body_vel > velocity_threshold), + torch.where(t_max < mode_time, t_min, 0), + stance_cmd_reward, + ) + return torch.sum(reward, dim=1) + + +def base_angular_velocity_reward(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, std: float) -> torch.Tensor: + """Reward tracking of angular velocity commands (yaw) using abs exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + # compute the error + target = env.command_manager.get_command("base_velocity")[:, 2] + ang_vel_error = torch.linalg.norm((target - asset.data.root_ang_vel_b[:, 2]).unsqueeze(1), dim=1) + return torch.exp(-ang_vel_error / std) + + +def base_linear_velocity_reward( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, std: float, ramp_at_vel: float = 1.0, ramp_rate: float = 0.5 +) -> torch.Tensor: + """Reward tracking of linear velocity commands (xy axes) using abs exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + # compute the error + target = env.command_manager.get_command("base_velocity")[:, :2] + lin_vel_error = torch.linalg.norm((target - asset.data.root_lin_vel_b[:, :2]), dim=1) + # fixed 1.0 multiple for tracking below the ramp_at_vel value, then scale by the rate above + vel_cmd_magnitude = torch.linalg.norm(target, dim=1) + velocity_scaling_multiple = torch.clamp(1.0 + ramp_rate * (vel_cmd_magnitude - ramp_at_vel), min=1.0) + return torch.exp(-lin_vel_error / std) * velocity_scaling_multiple + + +class GaitReward(ManagerTermBase): + """Gait enforcing reward term for quadrupeds. + + This reward penalizes contact timing differences between selected foot pairs defined in :attr:`synced_feet_pair_names` + to bias the policy towards a desired gait, i.e trotting, bounding, or pacing. Note that this reward is only for + quadrupedal gaits with two pairs of synchronized feet. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + """Initialize the term. + + Args: + cfg: The configuration of the reward. + env: The RL environment instance. + """ + super().__init__(cfg, env) + self.std: float = cfg.params["std"] + self.max_err: float = cfg.params["max_err"] + self.velocity_threshold: float = cfg.params["velocity_threshold"] + self.contact_sensor: ContactSensor = env.scene.sensors[cfg.params["sensor_cfg"].name] + self.asset: Articulation = env.scene[cfg.params["asset_cfg"].name] + # match foot body names with corresponding foot body ids + synced_feet_pair_names = cfg.params["synced_feet_pair_names"] + if ( + len(synced_feet_pair_names) != 2 + or len(synced_feet_pair_names[0]) != 2 + or len(synced_feet_pair_names[1]) != 2 + ): + raise ValueError("This reward only supports gaits with two pairs of synchronized feet, like trotting.") + synced_feet_pair_0 = self.contact_sensor.find_bodies(synced_feet_pair_names[0])[0] + synced_feet_pair_1 = self.contact_sensor.find_bodies(synced_feet_pair_names[1])[0] + self.synced_feet_pairs = [synced_feet_pair_0, synced_feet_pair_1] + + def __call__( + self, + env: ManagerBasedRLEnv, + std: float, + max_err: float, + velocity_threshold: float, + synced_feet_pair_names, + asset_cfg: SceneEntityCfg, + sensor_cfg: SceneEntityCfg, + ) -> torch.Tensor: + """Compute the reward. + + This reward is defined as a multiplication between six terms where two of them enforce pair feet + being in sync and the other four rewards if all the other remaining pairs are out of sync + + Args: + env: The RL environment instance. + Returns: + The reward value. + """ + # for synchronous feet, the contact (air) times of two feet should match + sync_reward_0 = self._sync_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[0][1]) + sync_reward_1 = self._sync_reward_func(self.synced_feet_pairs[1][0], self.synced_feet_pairs[1][1]) + sync_reward = sync_reward_0 * sync_reward_1 + # for asynchronous feet, the contact time of one foot should match the air time of the other one + async_reward_0 = self._async_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[1][0]) + async_reward_1 = self._async_reward_func(self.synced_feet_pairs[0][1], self.synced_feet_pairs[1][1]) + async_reward_2 = self._async_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[1][1]) + async_reward_3 = self._async_reward_func(self.synced_feet_pairs[1][0], self.synced_feet_pairs[0][1]) + async_reward = async_reward_0 * async_reward_1 * async_reward_2 * async_reward_3 + # only enforce gait if cmd > 0 + cmd = torch.norm(env.command_manager.get_command("base_velocity"), dim=1) + body_vel = torch.linalg.norm(self.asset.data.root_lin_vel_b[:, :2], dim=1) + return torch.where( + torch.logical_or(cmd > 0.0, body_vel > self.velocity_threshold), sync_reward * async_reward, 0.0 + ) + + """ + Helper functions. + """ + + def _sync_reward_func(self, foot_0: int, foot_1: int) -> torch.Tensor: + """Reward synchronization of two feet.""" + air_time = self.contact_sensor.data.current_air_time + contact_time = self.contact_sensor.data.current_contact_time + # penalize the difference between the most recent air time and contact time of synced feet pairs. + se_air = torch.clip(torch.square(air_time[:, foot_0] - air_time[:, foot_1]), max=self.max_err**2) + se_contact = torch.clip(torch.square(contact_time[:, foot_0] - contact_time[:, foot_1]), max=self.max_err**2) + return torch.exp(-(se_air + se_contact) / self.std) + + def _async_reward_func(self, foot_0: int, foot_1: int) -> torch.Tensor: + """Reward anti-synchronization of two feet.""" + air_time = self.contact_sensor.data.current_air_time + contact_time = self.contact_sensor.data.current_contact_time + # penalize the difference between opposing contact modes air time of feet 1 to contact time of feet 2 + # and contact time of feet 1 to air time of feet 2) of feet pairs that are not in sync with each other. + se_act_0 = torch.clip(torch.square(air_time[:, foot_0] - contact_time[:, foot_1]), max=self.max_err**2) + se_act_1 = torch.clip(torch.square(contact_time[:, foot_0] - air_time[:, foot_1]), max=self.max_err**2) + return torch.exp(-(se_act_0 + se_act_1) / self.std) + + +def foot_clearance_reward( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, target_height: float, std: float, tanh_mult: float +) -> torch.Tensor: + """Reward the swinging feet for clearing a specified height off the ground""" + asset: RigidObject = env.scene[asset_cfg.name] + foot_z_target_error = torch.square(asset.data.body_pos_w[:, asset_cfg.body_ids, 2] - target_height) + foot_velocity_tanh = torch.tanh(tanh_mult * torch.norm(asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2)) + reward = foot_z_target_error * foot_velocity_tanh + return torch.exp(-torch.sum(reward, dim=1) / std) + + +## +# Regularization Penalties +## + + +def action_smoothness_penalty(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize large instantaneous changes in the network action output""" + return torch.linalg.norm((env.action_manager.action - env.action_manager.prev_action), dim=1) + + +def air_time_variance_penalty(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize variance in the amount of time each foot spends in the air/on the ground relative to each other""" + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + if contact_sensor.cfg.track_air_time is False: + raise RuntimeError("Activate ContactSensor's track_air_time!") + # compute the reward + last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids] + last_contact_time = contact_sensor.data.last_contact_time[:, sensor_cfg.body_ids] + return torch.var(torch.clip(last_air_time, max=0.5), dim=1) + torch.var( + torch.clip(last_contact_time, max=0.5), dim=1 + ) + + +# ! look into simplifying the kernel here; it's a little oddly complex +def base_motion_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize base vertical and roll/pitch velocity""" + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return 0.8 * torch.square(asset.data.root_lin_vel_b[:, 2]) + 0.2 * torch.sum( + torch.abs(asset.data.root_ang_vel_b[:, :2]), dim=1 + ) + + +def base_orientation_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize non-flat base orientation + + This is computed by penalizing the xy-components of the projected gravity vector. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + return torch.linalg.norm((asset.data.projected_gravity_b[:, :2]), dim=1) + + +def foot_slip_penalty( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, sensor_cfg: SceneEntityCfg, threshold: float +) -> torch.Tensor: + """Penalize foot planar (xy) slip when in contact with the ground""" + asset: RigidObject = env.scene[asset_cfg.name] + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + + # check if contact force is above threshold + net_contact_forces = contact_sensor.data.net_forces_w_history + is_contact = torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] > threshold + foot_planar_velocity = torch.linalg.norm(asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2) + + reward = is_contact * foot_planar_velocity + return torch.sum(reward, dim=1) + + +def joint_acceleration_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize joint accelerations on the articulation.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.linalg.norm((asset.data.joint_acc), dim=1) + + +def joint_position_penalty( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, stand_still_scale: float, velocity_threshold: float +) -> torch.Tensor: + """Penalize joint position error from default on the articulation.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + cmd = torch.linalg.norm(env.command_manager.get_command("base_velocity"), dim=1) + body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1) + reward = torch.linalg.norm((asset.data.joint_pos - asset.data.default_joint_pos), dim=1) + return torch.where(torch.logical_or(cmd > 0.0, body_vel > velocity_threshold), reward, stand_still_scale * reward) + + +def joint_torques_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize joint torques on the articulation.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.linalg.norm((asset.data.applied_torque), dim=1) + + +def joint_velocity_penalty(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize joint velocities on the articulation.""" + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + return torch.linalg.norm((asset.data.joint_vel), dim=1) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py new file mode 100644 index 00000000000..2e6816e9adb --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .curriculums import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py new file mode 100644 index 00000000000..f94cc781747 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/curriculums.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to create curriculum for the learning environment. + +The functions can be passed to the :class:`isaaclab.managers.CurriculumTermCfg` object to enable +the curriculum introduced by the function. +""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg +from isaaclab.terrains import TerrainImporter + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def terrain_levels_vel( + env: ManagerBasedRLEnv, env_ids: Sequence[int], asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Curriculum based on the distance the robot walked when commanded to move at a desired velocity. + + This term is used to increase the difficulty of the terrain when the robot walks far enough and decrease the + difficulty when the robot walks less than half of the distance required by the commanded velocity. + + .. note:: + It is only possible to use this term with the terrain type ``generator``. For further information + on different terrain types, check the :class:`isaaclab.terrains.TerrainImporter` class. + + Returns: + The mean terrain level for the given environment ids. + """ + # extract the used quantities (to enable type-hinting) + asset: Articulation = env.scene[asset_cfg.name] + terrain: TerrainImporter = env.scene.terrain + command = env.command_manager.get_command("base_velocity") + # compute the distance the robot walked + distance = torch.norm(asset.data.root_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2], dim=1) + # robots that walked far enough progress to harder terrains + move_up = distance > terrain.cfg.terrain_generator.size[0] / 2 + # robots that walked less than half of their required distance go to simpler terrains + move_down = distance < torch.norm(command[env_ids, :2], dim=1) * env.max_episode_length_s * 0.5 + move_down *= ~move_up + # update terrain levels + terrain.update_env_origins(env_ids, move_up, move_down) + # return the mean terrain level + return torch.mean(terrain.terrain_levels.float()) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py new file mode 100644 index 00000000000..6e6ee473d44 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py @@ -0,0 +1,111 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to define rewards for the learning environment. + +The functions can be passed to the :class:`isaaclab.managers.RewardTermCfg` object to +specify the reward function and its parameters. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensor +from isaaclab.utils.math import quat_apply_inverse, yaw_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def feet_air_time( + env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float +) -> torch.Tensor: + """Reward long steps taken by the feet using L2-kernel. + + This function rewards the agent for taking steps that are longer than a threshold. This helps ensure + that the robot lifts its feet off the ground and takes steps. The reward is computed as the sum of + the time for which the feet are in the air. + + If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero. + """ + # extract the used quantities (to enable type-hinting) + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + # compute the reward + first_contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids] + last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids] + reward = torch.sum((last_air_time - threshold) * first_contact, dim=1) + # no reward for zero command + reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1 + return reward + + +def feet_air_time_positive_biped(env, command_name: str, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Reward long steps taken by the feet for bipeds. + + This function rewards the agent for taking steps up to a specified threshold and also keep one foot at + a time in the air. + + If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero. + """ + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + # compute the reward + air_time = contact_sensor.data.current_air_time[:, sensor_cfg.body_ids] + contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids] + in_contact = contact_time > 0.0 + in_mode_time = torch.where(in_contact, contact_time, air_time) + single_stance = torch.sum(in_contact.int(), dim=1) == 1 + reward = torch.min(torch.where(single_stance.unsqueeze(-1), in_mode_time, 0.0), dim=1)[0] + reward = torch.clamp(reward, max=threshold) + # no reward for zero command + reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1 + return reward + + +def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Penalize feet sliding. + + This function penalizes the agent for sliding its feet on the ground. The reward is computed as the + norm of the linear velocity of the feet multiplied by a binary contact sensor. This ensures that the + agent is penalized only when the feet are in contact with the ground. + """ + # Penalize feet sliding + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + contacts = contact_sensor.data.net_forces_w_history[:, :, sensor_cfg.body_ids, :].norm(dim=-1).max(dim=1)[0] > 1.0 + asset = env.scene[asset_cfg.name] + + body_vel = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2] + reward = torch.sum(body_vel.norm(dim=-1) * contacts, dim=1) + return reward + + +def track_lin_vel_xy_yaw_frame_exp( + env, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of linear velocity commands (xy axes) in the gravity aligned robot frame using exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset = env.scene[asset_cfg.name] + vel_yaw = quat_apply_inverse(yaw_quat(asset.data.root_quat_w), asset.data.root_lin_vel_w[:, :3]) + lin_vel_error = torch.sum( + torch.square(env.command_manager.get_command(command_name)[:, :2] - vel_yaw[:, :2]), dim=1 + ) + return torch.exp(-lin_vel_error / std**2) + + +def track_ang_vel_z_world_exp( + env, command_name: str, std: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Reward tracking of angular velocity commands (yaw) in world frame using exponential kernel.""" + # extract the used quantities (to enable type-hinting) + asset = env.scene[asset_cfg.name] + ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_w[:, 2]) + return torch.exp(-ang_vel_error / std**2) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py new file mode 100644 index 00000000000..c9fac4c4e08 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def terrain_out_of_bounds( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), distance_buffer: float = 3.0 +) -> torch.Tensor: + """Terminate when the actor move too close to the edge of the terrain. + + If the actor moves too close to the edge of the terrain, the termination is activated. The distance + to the edge of the terrain is calculated based on the size of the terrain and the distance buffer. + """ + if env.scene.cfg.terrain.terrain_type == "plane": + return False # we have infinite terrain because it is a plane + elif env.scene.cfg.terrain.terrain_type == "generator": + # obtain the size of the sub-terrains + terrain_gen_cfg = env.scene.terrain.cfg.terrain_generator + grid_width, grid_length = terrain_gen_cfg.size + n_rows, n_cols = terrain_gen_cfg.num_rows, terrain_gen_cfg.num_cols + border_width = terrain_gen_cfg.border_width + # compute the size of the map + map_width = n_rows * grid_width + 2 * border_width + map_height = n_cols * grid_length + 2 * border_width + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # check if the agent is out of bounds + x_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 0]) > 0.5 * map_width - distance_buffer + y_out_of_bounds = torch.abs(asset.data.root_pos_w[:, 1]) > 0.5 * map_height - distance_buffer + return torch.logical_or(x_out_of_bounds, y_out_of_bounds) + else: + raise ValueError("Received unsupported terrain type, must be either 'plane' or 'generator'.") diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py new file mode 100644 index 00000000000..2874f0adb57 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -0,0 +1,334 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +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, RayCasterCfg, patterns +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp + +## +# Pre-defined configs +## +from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG # isort: skip + + +## +# Scene definition +## + + +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with a legged robot.""" + + # ground terrain + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=ROUGH_TERRAINS_CFG, + max_init_terrain_level=5, + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + debug_vis=False, + ) + # robots + robot: ArticulationCfg = MISSING + # sensors + height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + attach_yaw_only=True, + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=False, + mesh_prim_paths=["/World/ground"], + ) + contact_forces = ContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) + # lights + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command specifications for the MDP.""" + + base_velocity = mdp.UniformVelocityCommandCfg( + asset_name="robot", + resampling_time_range=(10.0, 10.0), + rel_standing_envs=0.02, + rel_heading_envs=1.0, + heading_command=True, + heading_control_stiffness=0.5, + debug_vis=True, + ranges=mdp.UniformVelocityCommandCfg.Ranges( + lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi) + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2)) + projected_gravity = ObsTerm( + func=mdp.projected_gravity, + noise=Unoise(n_min=-0.05, n_max=0.05), + ) + velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) + joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) + actions = ObsTerm(func=mdp.last_action) + height_scan = ObsTerm( + func=mdp.height_scan, + params={"sensor_cfg": SceneEntityCfg("height_scanner")}, + noise=Unoise(n_min=-0.1, n_max=0.1), + clip=(-1.0, 1.0), + ) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # startup + physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 0.8), + "dynamic_friction_range": (0.6, 0.6), + "restitution_range": (0.0, 0.0), + "num_buckets": 64, + }, + ) + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "mass_distribution_params": (-5.0, 5.0), + "operation": "add", + }, + ) + + base_com = EventTerm( + func=mdp.randomize_rigid_body_com, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)}, + }, + ) + + # reset + base_external_force_torque = EventTerm( + func=mdp.apply_external_force_torque, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "force_range": (0.0, 0.0), + "torque_range": (-0.0, 0.0), + }, + ) + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (-0.5, 0.5), + "y": (-0.5, 0.5), + "z": (-0.5, 0.5), + "roll": (-0.5, 0.5), + "pitch": (-0.5, 0.5), + "yaw": (-0.5, 0.5), + }, + }, + ) + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (0.5, 1.5), + "velocity_range": (0.0, 0.0), + }, + ) + + # interval + push_robot = EventTerm( + func=mdp.push_by_setting_velocity, + mode="interval", + interval_range_s=(10.0, 15.0), + params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}}, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # -- task + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_exp, weight=1.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)} + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_exp, weight=0.5, params={"command_name": "base_velocity", "std": math.sqrt(0.25)} + ) + # -- penalties + lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=-2.0) + ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=-0.05) + dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5) + dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.01) + feet_air_time = RewTerm( + func=mdp.feet_air_time, + weight=0.125, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*FOOT"), + "command_name": "base_velocity", + "threshold": 0.5, + }, + ) + undesired_contacts = RewTerm( + func=mdp.undesired_contacts, + weight=-1.0, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*THIGH"), "threshold": 1.0}, + ) + # -- optional penalties + flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0) + dof_pos_limits = RewTerm(func=mdp.joint_pos_limits, weight=0.0) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + base_contact = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="base"), "threshold": 1.0}, + ) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + terrain_levels = CurrTerm(func=mdp.terrain_levels_vel) + + +## +# Environment configuration +## + + +@configclass +class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the locomotion velocity-tracking environment.""" + + # Scene settings + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 0.005 + self.sim.render_interval = self.decimation + self.sim.physics_material = self.scene.terrain.physics_material + self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 + # update sensor update periods + # we tick all the sensors based on the smallest update period (physics update period) + if self.scene.height_scanner is not None: + self.scene.height_scanner.update_period = self.decimation * self.sim.dt + if self.scene.contact_forces is not None: + self.scene.contact_forces.update_period = self.sim.dt + + # check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator + # this generates terrains with increasing difficulty and is useful for training + if getattr(self.curriculum, "terrain_levels", None) is not None: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = True + else: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/__init__.py new file mode 100644 index 00000000000..3d76930acdd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manipulation environments for fixed-arm robots.""" + +from .reach import * # noqa diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py new file mode 100644 index 00000000000..addbe3790c3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manipulation environments to open drawers in a cabinet.""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py new file mode 100644 index 00000000000..e7f68d005a7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -0,0 +1,285 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +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 FrameTransformerCfg +from isaaclab.sensors.frame_transformer import OffsetCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +FRAME_MARKER_SMALL_CFG = FRAME_MARKER_CFG.copy() +FRAME_MARKER_SMALL_CFG.markers["frame"].scale = (0.10, 0.10, 0.10) + + +## +# Scene definition +## + + +@configclass +class CabinetSceneCfg(InteractiveSceneCfg): + """Configuration for the cabinet scene with a robot and a cabinet. + + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the robot and end-effector frames + """ + + # robots, Will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # End-effector, Will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + + cabinet = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Cabinet", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Sektion_Cabinet/sektion_cabinet_instanceable.usd", + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.8, 0, 0.4), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "door_left_joint": 0.0, + "door_right_joint": 0.0, + "drawer_bottom_joint": 0.0, + "drawer_top_joint": 0.0, + }, + ), + actuators={ + "drawers": ImplicitActuatorCfg( + joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=10.0, + damping=1.0, + ), + "doors": ImplicitActuatorCfg( + joint_names_expr=["door_left_joint", "door_right_joint"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=10.0, + damping=2.5, + ), + }, + ) + + # Frame definitions for the cabinet. + cabinet_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Cabinet/sektion", + debug_vis=True, + visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/CabinetFrameTransformer"), + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Cabinet/drawer_handle_top", + name="drawer_handle_top", + offset=OffsetCfg( + pos=(0.305, 0.0, 0.01), + rot=(0.5, 0.5, -0.5, -0.5), # align with end-effector frame + ), + ), + ], + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(), + spawn=sim_utils.GroundPlaneCfg(), + collision_group=-1, + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action: mdp.JointPositionActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + cabinet_joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + cabinet_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + rel_ee_drawer_distance = ObsTerm(func=mdp.rel_ee_drawer_distance) + + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 1.25), + "dynamic_friction_range": (0.8, 1.25), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + cabinet_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("cabinet", body_names="drawer_handle_top"), + "static_friction_range": (1.0, 1.25), + "dynamic_friction_range": (1.25, 1.5), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "position_range": (-0.1, 0.1), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # 1. Approach the handle + approach_ee_handle = RewTerm(func=mdp.approach_ee_handle, weight=2.0, params={"threshold": 0.2}) + align_ee_handle = RewTerm(func=mdp.align_ee_handle, weight=0.5) + + # 2. Grasp the handle + approach_gripper_handle = RewTerm(func=mdp.approach_gripper_handle, weight=5.0, params={"offset": MISSING}) + align_grasp_around_handle = RewTerm(func=mdp.align_grasp_around_handle, weight=0.125) + grasp_handle = RewTerm( + func=mdp.grasp_handle, + weight=0.5, + params={ + "threshold": 0.03, + "open_joint_pos": MISSING, + "asset_cfg": SceneEntityCfg("robot", joint_names=MISSING), + }, + ) + + # 3. Open the drawer + open_drawer_bonus = RewTerm( + func=mdp.open_drawer_bonus, + weight=7.5, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + multi_stage_open_drawer = RewTerm( + func=mdp.multi_stage_open_drawer, + weight=1.0, + params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, + ) + + # 4. Penalize actions for cosmetic reasons + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1e-2) + joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-0.0001) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +## +# Environment configuration +## + + +@configclass +class CabinetEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the cabinet environment.""" + + # Scene settings + scene: CabinetSceneCfg = CabinetSceneCfg(num_envs=4096, env_spacing=2.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 1 + self.episode_length_s = 8.0 + self.viewer.eye = (-2.0, 2.0, 2.0) + self.viewer.lookat = (0.8, 0.0, 0.5) + # simulation settings + self.sim.dt = 1 / 60 # 60Hz + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py new file mode 100644 index 00000000000..58f49db7419 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the cabinet environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py new file mode 100644 index 00000000000..f0cdff8f972 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Open-Drawer-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCabinetEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CabinetPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Open-Drawer-Franka-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCabinetEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:CabinetPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + + +## +# Inverse Kinematics - Absolute Pose Control +## + +gym.register( + id="Isaac-Open-Drawer-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:FrankaCabinetEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Open-Drawer-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_rel_env_cfg:FrankaCabinetEnvCfg", + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..69fbb2a236f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 5.0 + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False + load_path: '' + + config: + name: franka_open_drawer + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: False + normalize_value: False + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1 + normalize_advantage: False + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 200 + max_epochs: 400 + save_best_after: 50 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.001 + truncate_grads: True + e_clip: 0.2 + horizon_length: 96 + minibatch_size: 4096 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..8fcc9a44856 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class CabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 96 + max_iterations = 400 + save_interval = 50 + experiment_name = "franka_open_drawer" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=1e-3, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.02, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..dedde5178a2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 96 + learning_epochs: 5 + mini_batches: 96 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.001 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "franka_open_drawer" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 38400 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py new file mode 100644 index 00000000000..40af7c9b8f9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCabinetEnvCfg(joint_pos_env_cfg.FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaCabinetEnvCfg_PLAY(FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py new file mode 100644 index 00000000000..3e901be004e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCabinetEnvCfg(joint_pos_env_cfg.FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaCabinetEnvCfg_PLAY(FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py new file mode 100644 index 00000000000..3fc16da76a5 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.cabinet import mdp + +from isaaclab_tasks.manager_based.manipulation.cabinet.cabinet_env_cfg import ( # isort: skip + FRAME_MARKER_SMALL_CFG, + CabinetEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaCabinetEnvCfg(CabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set Actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + scale=1.0, + use_default_offset=True, + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + + # Listens to the required transforms + # IMPORTANT: The order of the frames in the list is important. The first frame is the tool center point (TCP) + # the other frames are the fingers + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/EndEffectorFrameTransformer"), + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + name="ee_tcp", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.1034), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_leftfinger", + name="tool_leftfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + name="tool_rightfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + ], + ) + + # override rewards + self.rewards.approach_gripper_handle.params["offset"] = 0.04 + self.rewards.grasp_handle.params["open_joint_pos"] = 0.04 + self.rewards.grasp_handle.params["asset_cfg"].joint_names = ["panda_finger_.*"] + + +@configclass +class FrankaCabinetEnvCfg_PLAY(FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py new file mode 100644 index 00000000000..1b166d05a3a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the cabinet environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py new file mode 100644 index 00000000000..37568d47963 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import ArticulationData +from isaaclab.sensors import FrameTransformerData + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def rel_ee_object_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + object_data: ArticulationData = env.scene["object"].data + + return object_data.root_pos_w - ee_tf_data.target_pos_w[..., 0, :] + + +def rel_ee_drawer_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + cabinet_tf_data: FrameTransformerData = env.scene["cabinet_frame"].data + + return cabinet_tf_data.target_pos_w[..., 0, :] - ee_tf_data.target_pos_w[..., 0, :] + + +def fingertips_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the fingertips relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + fingertips_pos = ee_tf_data.target_pos_w[..., 1:, :] - env.scene.env_origins.unsqueeze(1) + + return fingertips_pos.view(env.num_envs, -1) + + +def ee_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the end-effector relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_pos = ee_tf_data.target_pos_w[..., 0, :] - env.scene.env_origins + + return ee_pos + + +def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tensor: + """The orientation of the end-effector in the environment frame. + + If :attr:`make_quat_unique` is True, the quaternion is made unique by ensuring the real part is positive. + """ + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_quat = ee_tf_data.target_quat_w[..., 0, :] + # make first element of quaternion positive + return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py new file mode 100644 index 00000000000..a5dd794c4c7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/rewards.py @@ -0,0 +1,167 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import matrix_from_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def approach_ee_handle(env: ManagerBasedRLEnv, threshold: float) -> torch.Tensor: + r"""Reward the robot for reaching the drawer handle using inverse-square law. + + It uses a piecewise function to reward the robot for reaching the handle. + + .. math:: + + reward = \begin{cases} + 2 * (1 / (1 + distance^2))^2 & \text{if } distance \leq threshold \\ + (1 / (1 + distance^2))^2 & \text{otherwise} + \end{cases} + + """ + ee_tcp_pos = env.scene["ee_frame"].data.target_pos_w[..., 0, :] + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + + # Compute the distance of the end-effector to the handle + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + + # Reward the robot for reaching the handle + reward = 1.0 / (1.0 + distance**2) + reward = torch.pow(reward, 2) + return torch.where(distance <= threshold, 2 * reward, reward) + + +def align_ee_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Reward for aligning the end-effector with the handle. + + The reward is based on the alignment of the gripper with the handle. It is computed as follows: + + .. math:: + + reward = 0.5 * (align_z^2 + align_x^2) + + where :math:`align_z` is the dot product of the z direction of the gripper and the -x direction of the handle + and :math:`align_x` is the dot product of the x direction of the gripper and the -y direction of the handle. + """ + ee_tcp_quat = env.scene["ee_frame"].data.target_quat_w[..., 0, :] + handle_quat = env.scene["cabinet_frame"].data.target_quat_w[..., 0, :] + + ee_tcp_rot_mat = matrix_from_quat(ee_tcp_quat) + handle_mat = matrix_from_quat(handle_quat) + + # get current x and y direction of the handle + handle_x, handle_y = handle_mat[..., 0], handle_mat[..., 1] + # get current x and z direction of the gripper + ee_tcp_x, ee_tcp_z = ee_tcp_rot_mat[..., 0], ee_tcp_rot_mat[..., 2] + + # make sure gripper aligns with the handle + # in this case, the z direction of the gripper should be close to the -x direction of the handle + # and the x direction of the gripper should be close to the -y direction of the handle + # dot product of z and x should be large + align_z = torch.bmm(ee_tcp_z.unsqueeze(1), -handle_x.unsqueeze(-1)).squeeze(-1).squeeze(-1) + align_x = torch.bmm(ee_tcp_x.unsqueeze(1), -handle_y.unsqueeze(-1)).squeeze(-1).squeeze(-1) + return 0.5 * (torch.sign(align_z) * align_z**2 + torch.sign(align_x) * align_x**2) + + +def align_grasp_around_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Bonus for correct hand orientation around the handle. + + The correct hand orientation is when the left finger is above the handle and the right finger is below the handle. + """ + # Target object position: (num_envs, 3) + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + # Fingertips position: (num_envs, n_fingertips, 3) + ee_fingertips_w = env.scene["ee_frame"].data.target_pos_w[..., 1:, :] + lfinger_pos = ee_fingertips_w[..., 0, :] + rfinger_pos = ee_fingertips_w[..., 1, :] + + # Check if hand is in a graspable pose + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + + # bonus if left finger is above the drawer handle and right below + return is_graspable + + +def approach_gripper_handle(env: ManagerBasedRLEnv, offset: float = 0.04) -> torch.Tensor: + """Reward the robot's gripper reaching the drawer handle with the right pose. + + This function returns the distance of fingertips to the handle when the fingers are in a grasping orientation + (i.e., the left finger is above the handle and the right finger is below the handle). Otherwise, it returns zero. + """ + # Target object position: (num_envs, 3) + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + # Fingertips position: (num_envs, n_fingertips, 3) + ee_fingertips_w = env.scene["ee_frame"].data.target_pos_w[..., 1:, :] + lfinger_pos = ee_fingertips_w[..., 0, :] + rfinger_pos = ee_fingertips_w[..., 1, :] + + # Compute the distance of each finger from the handle + lfinger_dist = torch.abs(lfinger_pos[:, 2] - handle_pos[:, 2]) + rfinger_dist = torch.abs(rfinger_pos[:, 2] - handle_pos[:, 2]) + + # Check if hand is in a graspable pose + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + + return is_graspable * ((offset - lfinger_dist) + (offset - rfinger_dist)) + + +def grasp_handle( + env: ManagerBasedRLEnv, threshold: float, open_joint_pos: float, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward for closing the fingers when being close to the handle. + + The :attr:`threshold` is the distance from the handle at which the fingers should be closed. + The :attr:`open_joint_pos` is the joint position when the fingers are open. + + Note: + It is assumed that zero joint position corresponds to the fingers being closed. + """ + ee_tcp_pos = env.scene["ee_frame"].data.target_pos_w[..., 0, :] + handle_pos = env.scene["cabinet_frame"].data.target_pos_w[..., 0, :] + gripper_joint_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids] + + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + is_close = distance <= threshold + + return is_close * torch.sum(open_joint_pos - gripper_joint_pos, dim=-1) + + +def open_drawer_bonus(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Bonus for opening the drawer given by the joint position of the drawer. + + The bonus is given when the drawer is open. If the grasp is around the handle, the bonus is doubled. + """ + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + is_graspable = align_grasp_around_handle(env).float() + + return (is_graspable + 1.0) * drawer_pos + + +def multi_stage_open_drawer(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Multi-stage bonus for opening the drawer. + + Depending on the drawer's position, the reward is given in three stages: easy, medium, and hard. + This helps the agent to learn to open the drawer in a controlled manner. + """ + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + is_graspable = align_grasp_around_handle(env).float() + + open_easy = (drawer_pos > 0.01) * 0.5 + open_medium = (drawer_pos > 0.2) * is_graspable + open_hard = (drawer_pos > 0.3) * is_graspable + + return open_easy + open_medium + open_hard diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py new file mode 100644 index 00000000000..1a6b49c6649 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""In-hand object reorientation environment. + +These environments are based on the `dexterous cube manipulation`_ environments +provided in IsaacGymEnvs repository from NVIDIA. However, they contain certain +modifications and additional features. + +.. _dexterous cube manipulation: https://github.com/NVIDIA-Omniverse/IsaacGymEnvs/blob/main/isaacgymenvs/tasks/allegro_hand.py + +""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py new file mode 100644 index 00000000000..682e37d0035 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for in-hand manipulation environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py new file mode 100644 index 00000000000..d1fa1138a4c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/__init__.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Full kinematic state observations. +## + +gym.register( + id="Isaac-Repose-Cube-Allegro-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.allegro_env_cfg:AllegroCubeEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AllegroCubePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Repose-Cube-Allegro-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.allegro_env_cfg:AllegroCubeEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AllegroCubePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +## +# Kinematic state observations without velocity information. +## + +gym.register( + id="Isaac-Repose-Cube-Allegro-NoVelObs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.allegro_env_cfg:AllegroCubeNoVelObsEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AllegroCubeNoVelObsPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Repose-Cube-Allegro-NoVelObs-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.allegro_env_cfg:AllegroCubeNoVelObsEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:AllegroCubeNoVelObsPPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..24b4540060e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,85 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 5.0 + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + + mlp: + units: [512, 256, 128] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False + load_path: '' + + config: + name: allegro_cube + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 0.1 + normalize_advantage: True + gamma: 0.998 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + schedule_type: standard + kl_threshold: 0.016 + score_to_win: 100000 + max_epochs: 5000 + save_best_after: 500 + save_frequency: 200 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.002 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 16384 # 32768 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0005 + + player: + #render: True + deterministic: True + games_num: 100000 + print_stats: True diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..dbf9c99cd01 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,47 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class AllegroCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 5000 + save_interval = 50 + experiment_name = "allegro_cube" + empirical_normalization = True + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.002, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=0.001, + schedule="adaptive", + gamma=0.998, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class AllegroCubeNoVelObsPPORunnerCfg(AllegroCubePPORunnerCfg): + experiment_name = "allegro_cube_no_vel_obs" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..a7763370650 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [512, 256, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 12 + discount_factor: 0.998 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.016 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.002 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.1 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "allegro_cube" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 120000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py new file mode 100644 index 00000000000..7a8a03dfcde --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/allegro_env_cfg.py @@ -0,0 +1,69 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.inhand.inhand_env_cfg as inhand_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets import ALLEGRO_HAND_CFG # isort: skip + + +@configclass +class AllegroCubeEnvCfg(inhand_env_cfg.InHandObjectEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to allegro hand + self.scene.robot = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class AllegroCubeEnvCfg_PLAY(AllegroCubeEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove termination due to timeouts + self.terminations.time_out = None + + +## +# Environment configuration with no velocity observations. +## + + +@configclass +class AllegroCubeNoVelObsEnvCfg(AllegroCubeEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch observation group to no velocity group + self.observations.policy = inhand_env_cfg.ObservationsCfg.NoVelocityKinematicObsGroupCfg() + + +@configclass +class AllegroCubeNoVelObsEnvCfg_PLAY(AllegroCubeNoVelObsEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove termination due to timeouts + self.terminations.time_out = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py new file mode 100644 index 00000000000..6ece6ef52de --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/inhand_env_cfg.py @@ -0,0 +1,351 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING + +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.sim.simulation_cfg import PhysxCfg, SimulationCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveGaussianNoiseCfg as Gnoise + +import isaaclab_tasks.manager_based.manipulation.inhand.mdp as mdp + +## +# Scene definition +## + + +@configclass +class InHandObjectSceneCfg(InteractiveSceneCfg): + """Configuration for a scene with an object and a dexterous hand.""" + + # robots + robot: ArticulationCfg = MISSING + + # objects + object: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + mass_props=sim_utils.MassPropertiesCfg(density=400.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.19, 0.56), rot=(1.0, 0.0, 0.0, 0.0)), + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.95, 0.95, 0.95), intensity=1000.0), + ) + + dome_light = AssetBaseCfg( + prim_path="/World/domeLight", + spawn=sim_utils.DomeLightCfg(color=(0.02, 0.02, 0.02), intensity=1000.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command specifications for the MDP.""" + + object_pose = mdp.InHandReOrientationCommandCfg( + asset_name="object", + init_pos_offset=(0.0, 0.0, -0.04), + update_goal_on_success=True, + orientation_success_threshold=0.1, + make_quat_unique=False, + marker_pos_offset=(-0.2, -0.06, 0.08), + debug_vis=True, + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = mdp.EMAJointPositionToLimitsActionCfg( + asset_name="robot", + joint_names=[".*"], + alpha=0.95, + rescale_to_limits=True, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class KinematicObsGroupCfg(ObsGroup): + """Observations with full-kinematic state information. + + This does not include acceleration or force information. + """ + + # observation terms (order preserved) + # -- robot terms + joint_pos = ObsTerm(func=mdp.joint_pos_limit_normalized, noise=Gnoise(std=0.005)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, scale=0.2, noise=Gnoise(std=0.01)) + + # -- object terms + object_pos = ObsTerm( + func=mdp.root_pos_w, noise=Gnoise(std=0.002), params={"asset_cfg": SceneEntityCfg("object")} + ) + object_quat = ObsTerm( + func=mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object"), "make_quat_unique": False} + ) + object_lin_vel = ObsTerm( + func=mdp.root_lin_vel_w, noise=Gnoise(std=0.002), params={"asset_cfg": SceneEntityCfg("object")} + ) + object_ang_vel = ObsTerm( + func=mdp.root_ang_vel_w, + scale=0.2, + noise=Gnoise(std=0.002), + params={"asset_cfg": SceneEntityCfg("object")}, + ) + + # -- command terms + goal_pose = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) + goal_quat_diff = ObsTerm( + func=mdp.goal_quat_diff, + params={"asset_cfg": SceneEntityCfg("object"), "command_name": "object_pose", "make_quat_unique": False}, + ) + + # -- action terms + last_action = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + @configclass + class NoVelocityKinematicObsGroupCfg(KinematicObsGroupCfg): + """Observations with partial kinematic state information. + + In contrast to the full-kinematic state group, this group does not include velocity information + about the robot joints and the object root frame. This is useful for tasks where velocity information + is not available or has a lot of noise. + """ + + def __post_init__(self): + # call parent post init + super().__post_init__() + # set unused terms to None + self.joint_vel = None + self.object_lin_vel = None + self.object_ang_vel = None + + # observation groups + policy: KinematicObsGroupCfg = KinematicObsGroupCfg() + + +@configclass +class EventCfg: + """Configuration for randomization.""" + + # startup + # -- robot + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (0.7, 1.3), + "restitution_range": (0.0, 0.0), + "num_buckets": 250, + }, + ) + robot_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "mass_distribution_params": (0.95, 1.05), + "operation": "scale", + }, + ) + robot_joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "stiffness_distribution_params": (0.3, 3.0), # default: 3.0 + "damping_distribution_params": (0.75, 1.5), # default: 0.1 + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + # -- object + object_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("object", body_names=".*"), + "static_friction_range": (0.7, 1.3), + "dynamic_friction_range": (0.7, 1.3), + "restitution_range": (0.0, 0.0), + "num_buckets": 250, + }, + ) + object_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("object"), + "mass_distribution_params": (0.4, 1.6), + "operation": "scale", + }, + ) + + # reset + reset_object = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": [-0.01, 0.01], "y": [-0.01, 0.01], "z": [-0.01, 0.01]}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object", body_names=".*"), + }, + ) + reset_robot_joints = EventTerm( + func=mdp.reset_joints_within_limits_range, + mode="reset", + params={ + "position_range": {".*": [0.2, 0.2]}, + "velocity_range": {".*": [0.0, 0.0]}, + "use_default_offset": True, + "operation": "scale", + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # -- task + # track_pos_l2 = RewTerm( + # func=mdp.track_pos_l2, + # weight=-10.0, + # params={"object_cfg": SceneEntityCfg("object"), "command_name": "object_pose"}, + # ) + track_orientation_inv_l2 = RewTerm( + func=mdp.track_orientation_inv_l2, + weight=1.0, + params={"object_cfg": SceneEntityCfg("object"), "rot_eps": 0.1, "command_name": "object_pose"}, + ) + success_bonus = RewTerm( + func=mdp.success_bonus, + weight=250.0, + params={"object_cfg": SceneEntityCfg("object"), "command_name": "object_pose"}, + ) + + # -- penalties + joint_vel_l2 = RewTerm(func=mdp.joint_vel_l2, weight=-2.5e-5) + action_l2 = RewTerm(func=mdp.action_l2, weight=-0.0001) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.01) + + # -- optional penalties (these are disabled by default) + # object_away_penalty = RewTerm( + # func=mdp.is_terminated_term, + # weight=-0.0, + # params={"term_keys": "object_out_of_reach"}, + # ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + max_consecutive_success = DoneTerm( + func=mdp.max_consecutive_success, params={"num_success": 50, "command_name": "object_pose"} + ) + + object_out_of_reach = DoneTerm(func=mdp.object_away_from_robot, params={"threshold": 0.3}) + + # object_out_of_reach = DoneTerm( + # func=mdp.object_away_from_goal, params={"threshold": 0.24, "command_name": "object_pose"} + # ) + + +## +# Environment configuration +## + + +@configclass +class InHandObjectEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the in hand reorientation environment.""" + + # Scene settings + scene: InHandObjectSceneCfg = InHandObjectSceneCfg(num_envs=8192, env_spacing=0.6) + # Simulation settings + sim: SimulationCfg = SimulationCfg( + physics_material=RigidBodyMaterialCfg( + static_friction=1.0, + dynamic_friction=1.0, + ), + physx=PhysxCfg( + bounce_threshold_velocity=0.2, + gpu_max_rigid_contact_count=2**20, + gpu_max_rigid_patch_count=2**23, + ), + ) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1.0 / 120.0 + self.sim.render_interval = self.decimation + # change viewer settings + self.viewer.eye = (2.0, 2.0, 2.0) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py new file mode 100644 index 00000000000..e8312f6bf15 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/__init__.py @@ -0,0 +1,19 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the in-hand manipulation environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .commands import * # noqa: F401, F403 +from .events import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py new file mode 100644 index 00000000000..cd5edcfe551 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command terms for 3D orientation goals.""" + +from .commands_cfg import InHandReOrientationCommandCfg # noqa: F401 +from .orientation_command import InHandReOrientationCommand # noqa: F401 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py new file mode 100644 index 00000000000..ee3db710905 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/commands_cfg.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.managers import CommandTermCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from .orientation_command import InHandReOrientationCommand + + +@configclass +class InHandReOrientationCommandCfg(CommandTermCfg): + """Configuration for the uniform 3D orientation command term. + + Please refer to the :class:`InHandReOrientationCommand` class for more details. + """ + + class_type: type = InHandReOrientationCommand + resampling_time_range: tuple[float, float] = (1e6, 1e6) # no resampling based on time + + asset_name: str = MISSING + """Name of the asset in the environment for which the commands are generated.""" + + init_pos_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position offset of the asset from its default position. + + This is used to account for the offset typically present in the object's default position + so that the object is spawned at a height above the robot's palm. When the position command + is generated, the object's default position is used as the reference and the offset specified + is added to it to get the desired position of the object. + """ + + make_quat_unique: bool = MISSING + """Whether to make the quaternion unique or not. + + If True, the quaternion is made unique by ensuring the real part is positive. + """ + + orientation_success_threshold: float = MISSING + """Threshold for the orientation error to consider the goal orientation to be reached.""" + + update_goal_on_success: bool = MISSING + """Whether to update the goal orientation when the goal orientation is reached.""" + + marker_pos_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Position offset of the marker from the object's desired position. + + This is useful to position the marker at a height above the object's desired position. + Otherwise, the marker may occlude the object in the visualization. + """ + + goal_pose_visualizer_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( + prim_path="/Visuals/Command/goal_marker", + markers={ + "goal": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(1.0, 1.0, 1.0), + ), + }, + ) + """The configuration for the goal pose visualization marker. Defaults to a DexCube marker.""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py new file mode 100644 index 00000000000..1c427e409f1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/commands/orientation_command.py @@ -0,0 +1,149 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing command generators for 3D orientation goals for objects.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import RigidObject +from isaaclab.managers import CommandTerm +from isaaclab.markers.visualization_markers import VisualizationMarkers + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + from .commands_cfg import InHandReOrientationCommandCfg + + +class InHandReOrientationCommand(CommandTerm): + """Command term that generates 3D pose commands for in-hand manipulation task. + + This command term generates 3D orientation commands for the object. The orientation commands + are sampled uniformly from the 3D orientation space. The position commands are the default + root state of the object. + + The constant position commands is to encourage that the object does not move during the task. + For instance, the object should not fall off the robot's palm. + + Unlike typical command terms, where the goals are resampled based on time, this command term + does not resample the goals based on time. Instead, the goals are resampled when the object + reaches the goal orientation. The goal orientation is considered to be reached when the + orientation error is below a certain threshold. + """ + + cfg: InHandReOrientationCommandCfg + """Configuration for the command term.""" + + def __init__(self, cfg: InHandReOrientationCommandCfg, env: ManagerBasedRLEnv): + """Initialize the command term class. + + Args: + cfg: The configuration parameters for the command term. + env: The environment object. + """ + # initialize the base class + super().__init__(cfg, env) + + # object + self.object: RigidObject = env.scene[cfg.asset_name] + + # create buffers to store the command + # -- command: (x, y, z) + init_pos_offset = torch.tensor(cfg.init_pos_offset, dtype=torch.float, device=self.device) + self.pos_command_e = self.object.data.default_root_state[:, :3] + init_pos_offset + self.pos_command_w = self.pos_command_e + self._env.scene.env_origins + # -- orientation: (w, x, y, z) + self.quat_command_w = torch.zeros(self.num_envs, 4, device=self.device) + self.quat_command_w[:, 0] = 1.0 # set the scalar component to 1.0 + + # -- unit vectors + self._X_UNIT_VEC = torch.tensor([1.0, 0, 0], device=self.device).repeat((self.num_envs, 1)) + self._Y_UNIT_VEC = torch.tensor([0, 1.0, 0], device=self.device).repeat((self.num_envs, 1)) + self._Z_UNIT_VEC = torch.tensor([0, 0, 1.0], device=self.device).repeat((self.num_envs, 1)) + + # -- metrics + self.metrics["orientation_error"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["position_error"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["consecutive_success"] = torch.zeros(self.num_envs, device=self.device) + + def __str__(self) -> str: + msg = "InHandManipulationCommandGenerator:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + return msg + + """ + Properties + """ + + @property + def command(self) -> torch.Tensor: + """The desired goal pose in the environment frame. Shape is (num_envs, 7).""" + return torch.cat((self.pos_command_e, self.quat_command_w), dim=-1) + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + # logs data + # -- compute the orientation error + self.metrics["orientation_error"] = math_utils.quat_error_magnitude( + self.object.data.root_quat_w, self.quat_command_w + ) + # -- compute the position error + self.metrics["position_error"] = torch.norm(self.object.data.root_pos_w - self.pos_command_w, dim=1) + # -- compute the number of consecutive successes + successes = self.metrics["orientation_error"] < self.cfg.orientation_success_threshold + self.metrics["consecutive_success"] += successes.float() + + def _resample_command(self, env_ids: Sequence[int]): + # sample new orientation targets + rand_floats = 2.0 * torch.rand((len(env_ids), 2), device=self.device) - 1.0 + # rotate randomly about x-axis and then y-axis + quat = math_utils.quat_mul( + math_utils.quat_from_angle_axis(rand_floats[:, 0] * torch.pi, self._X_UNIT_VEC[env_ids]), + math_utils.quat_from_angle_axis(rand_floats[:, 1] * torch.pi, self._Y_UNIT_VEC[env_ids]), + ) + # make sure the quaternion real-part is always positive + self.quat_command_w[env_ids] = math_utils.quat_unique(quat) if self.cfg.make_quat_unique else quat + + def _update_command(self): + # update the command if goal is reached + if self.cfg.update_goal_on_success: + # compute the goal resets + goal_resets = self.metrics["orientation_error"] < self.cfg.orientation_success_threshold + goal_reset_ids = goal_resets.nonzero(as_tuple=False).squeeze(-1) + # resample the goals + self._resample(goal_reset_ids) + + def _set_debug_vis_impl(self, debug_vis: TYPE_CHECKING): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first time + if not hasattr(self, "goal_pose_visualizer"): + self.goal_pose_visualizer = VisualizationMarkers(self.cfg.goal_pose_visualizer_cfg) + # set visibility + self.goal_pose_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_pose_visualizer"): + self.goal_pose_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # add an offset to the marker position to visualize the goal + marker_pos = self.pos_command_w + torch.tensor(self.cfg.marker_pos_offset, device=self.device) + marker_quat = self.quat_command_w + # visualize the goal marker + self.goal_pose_visualizer.visualize(translations=marker_pos, orientations=marker_quat) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py new file mode 100644 index 00000000000..7af11700117 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/events.py @@ -0,0 +1,189 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions specific to the in-hand dexterous manipulation environments.""" + + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING, Literal + +from isaaclab.assets import Articulation +from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg +from isaaclab.utils.math import sample_uniform + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +class reset_joints_within_limits_range(ManagerTermBase): + """Reset an articulation's joints to a random position in the given limit ranges. + + This function samples random values for the joint position and velocities from the given limit ranges. + The values are then set into the physics simulation. + + The parameters to the function are: + + * :attr:`position_range` - a dictionary of position ranges for each joint. The keys of the dictionary are the + joint names (or regular expressions) of the asset. + * :attr:`velocity_range` - a dictionary of velocity ranges for each joint. The keys of the dictionary are the + joint names (or regular expressions) of the asset. + * :attr:`use_default_offset` - a boolean flag to indicate if the ranges are offset by the default joint state. + Defaults to False. + * :attr:`asset_cfg` - the configuration of the asset to reset. Defaults to the entity named "robot" in the scene. + * :attr:`operation` - whether the ranges are scaled values of the joint limits, or absolute limits. + Defaults to "abs". + + The dictionary values are a tuple of the form ``(a, b)``. Based on the operation, these values are + interpreted differently: + + * If the operation is "abs", the values are the absolute minimum and maximum values for the joint, i.e. + the joint range becomes ``[a, b]``. + * If the operation is "scale", the values are the scaling factors for the joint limits, i.e. the joint range + becomes ``[a * min_joint_limit, b * max_joint_limit]``. + + If the ``a`` or the ``b`` value is ``None``, the joint limits are used instead. + + Note: + If the dictionary does not contain a key, the joint position or joint velocity is set to the default value for + that joint. + + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + # initialize the base class + super().__init__(cfg, env) + + # check if the cfg has the required parameters + if "position_range" not in cfg.params or "velocity_range" not in cfg.params: + raise ValueError( + "The term 'reset_joints_within_range' requires parameters: 'position_range' and 'velocity_range'." + f" Received: {list(cfg.params.keys())}." + ) + + # parse the parameters + asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) + use_default_offset = cfg.params.get("use_default_offset", False) + operation = cfg.params.get("operation", "abs") + # check if the operation is valid + if operation not in ["abs", "scale"]: + raise ValueError( + f"For event 'reset_joints_within_limits_range', unknown operation: '{operation}'." + " Please use 'abs' or 'scale'." + ) + + # extract the used quantities (to enable type-hinting) + self._asset: Articulation = env.scene[asset_cfg.name] + default_joint_pos = self._asset.data.default_joint_pos[0] + default_joint_vel = self._asset.data.default_joint_vel[0] + + # create buffers to store the joint position range + self._pos_ranges = self._asset.data.soft_joint_pos_limits[0].clone() + # parse joint position ranges + pos_joint_ids = [] + for joint_name, joint_range in cfg.params["position_range"].items(): + # find the joint ids + joint_ids = self._asset.find_joints(joint_name)[0] + pos_joint_ids.extend(joint_ids) + + # set the joint position ranges based on the given values + if operation == "abs": + if joint_range[0] is not None: + self._pos_ranges[joint_ids, 0] = joint_range[0] + if joint_range[1] is not None: + self._pos_ranges[joint_ids, 1] = joint_range[1] + elif operation == "scale": + if joint_range[0] is not None: + self._pos_ranges[joint_ids, 0] *= joint_range[0] + if joint_range[1] is not None: + self._pos_ranges[joint_ids, 1] *= joint_range[1] + else: + raise ValueError( + f"Unknown operation: '{operation}' for joint position ranges. Please use 'abs' or 'scale'." + ) + # add the default offset + if use_default_offset: + self._pos_ranges[joint_ids] += default_joint_pos[joint_ids].unsqueeze(1) + + # store the joint pos ids (used later to sample the joint positions) + self._pos_joint_ids = torch.tensor(pos_joint_ids, device=self._pos_ranges.device) + self._pos_ranges = self._pos_ranges[self._pos_joint_ids] + + # create buffers to store the joint velocity range + self._vel_ranges = torch.stack( + [-self._asset.data.soft_joint_vel_limits[0], self._asset.data.soft_joint_vel_limits[0]], dim=1 + ) + # parse joint velocity ranges + vel_joint_ids = [] + for joint_name, joint_range in cfg.params["velocity_range"].items(): + # find the joint ids + joint_ids = self._asset.find_joints(joint_name)[0] + vel_joint_ids.extend(joint_ids) + + # set the joint position ranges based on the given values + if operation == "abs": + if joint_range[0] is not None: + self._vel_ranges[joint_ids, 0] = joint_range[0] + if joint_range[1] is not None: + self._vel_ranges[joint_ids, 1] = joint_range[1] + elif operation == "scale": + if joint_range[0] is not None: + self._vel_ranges[joint_ids, 0] = joint_range[0] * self._vel_ranges[joint_ids, 0] + if joint_range[1] is not None: + self._vel_ranges[joint_ids, 1] = joint_range[1] * self._vel_ranges[joint_ids, 1] + else: + raise ValueError( + f"Unknown operation: '{operation}' for joint velocity ranges. Please use 'abs' or 'scale'." + ) + # add the default offset + if use_default_offset: + self._vel_ranges[joint_ids] += default_joint_vel[joint_ids].unsqueeze(1) + + # store the joint vel ids (used later to sample the joint positions) + self._vel_joint_ids = torch.tensor(vel_joint_ids, device=self._vel_ranges.device) + self._vel_ranges = self._vel_ranges[self._vel_joint_ids] + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: dict[str, tuple[float | None, float | None]], + velocity_range: dict[str, tuple[float | None, float | None]], + use_default_offset: bool = False, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + operation: Literal["abs", "scale"] = "abs", + ): + # get default joint state + joint_pos = self._asset.data.default_joint_pos[env_ids].clone() + joint_vel = self._asset.data.default_joint_vel[env_ids].clone() + + # sample random joint positions for each joint + if len(self._pos_joint_ids) > 0: + joint_pos_shape = (len(env_ids), len(self._pos_joint_ids)) + joint_pos[:, self._pos_joint_ids] = sample_uniform( + self._pos_ranges[:, 0], self._pos_ranges[:, 1], joint_pos_shape, device=joint_pos.device + ) + # clip the joint positions to the joint limits + joint_pos_limits = self._asset.data.soft_joint_pos_limits[0, self._pos_joint_ids] + joint_pos = joint_pos.clamp(joint_pos_limits[:, 0], joint_pos_limits[:, 1]) + + # sample random joint velocities for each joint + if len(self._vel_joint_ids) > 0: + joint_vel_shape = (len(env_ids), len(self._vel_joint_ids)) + joint_vel[:, self._vel_joint_ids] = sample_uniform( + self._vel_ranges[:, 0], self._vel_ranges[:, 1], joint_vel_shape, device=joint_vel.device + ) + # clip the joint velocities to the joint limits + joint_vel_limits = self._asset.data.soft_joint_vel_limits[0, self._vel_joint_ids] + joint_vel = joint_vel.clamp(-joint_vel_limits, joint_vel_limits) + + # set into the physics simulation + self._asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py new file mode 100644 index 00000000000..4e39bfd7260 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/observations.py @@ -0,0 +1,43 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions specific to the in-hand dexterous manipulation environments.""" + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import RigidObject +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from .commands import InHandReOrientationCommand + + +def goal_quat_diff( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, command_name: str, make_quat_unique: bool +) -> torch.Tensor: + """Goal orientation relative to the asset's root frame. + + The quaternion is represented as (w, x, y, z). The real part is always positive. + """ + # extract useful elements + asset: RigidObject = env.scene[asset_cfg.name] + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + + # obtain the orientations + goal_quat_w = command_term.command[:, 3:7] + asset_quat_w = asset.data.root_quat_w + + # compute quaternion difference + quat = math_utils.quat_mul(asset_quat_w, math_utils.quat_conjugate(goal_quat_w)) + # make sure the quaternion real-part is always positive + return math_utils.quat_unique(quat) if make_quat_unique else quat diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py new file mode 100644 index 00000000000..3297d95c6be --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/rewards.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions specific to the in-hand dexterous manipulation environments.""" + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import RigidObject +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from .commands import InHandReOrientationCommand + + +def success_bonus( + env: ManagerBasedRLEnv, command_name: str, object_cfg: SceneEntityCfg = SceneEntityCfg("object") +) -> torch.Tensor: + """Bonus reward for successfully reaching the goal. + + The object is considered to have reached the goal when the object orientation is within the threshold. + The reward is 1.0 if the object has reached the goal, otherwise 0.0. + + Args: + env: The environment object. + command_name: The command term to be used for extracting the goal. + object_cfg: The configuration for the scene entity. Default is "object". + """ + # extract useful elements + asset: RigidObject = env.scene[object_cfg.name] + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + + # obtain the goal orientation + goal_quat_w = command_term.command[:, 3:7] + # obtain the threshold for the orientation error + threshold = command_term.cfg.orientation_success_threshold + # calculate the orientation error + dtheta = math_utils.quat_error_magnitude(asset.data.root_quat_w, goal_quat_w) + + return dtheta <= threshold + + +def track_pos_l2( + env: ManagerBasedRLEnv, command_name: str, object_cfg: SceneEntityCfg = SceneEntityCfg("object") +) -> torch.Tensor: + """Reward for tracking the object position using the L2 norm. + + The reward is the distance between the object position and the goal position. + + Args: + env: The environment object. + command_term: The command term to be used for extracting the goal. + object_cfg: The configuration for the scene entity. Default is "object". + """ + # extract useful elements + asset: RigidObject = env.scene[object_cfg.name] + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + + # obtain the goal position + goal_pos_e = command_term.command[:, 0:3] + # obtain the object position in the environment frame + object_pos_e = asset.data.root_pos_w - env.scene.env_origins + + return torch.norm(goal_pos_e - object_pos_e, p=2, dim=-1) + + +def track_orientation_inv_l2( + env: ManagerBasedRLEnv, + command_name: str, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + rot_eps: float = 1e-3, +) -> torch.Tensor: + """Reward for tracking the object orientation using the inverse of the orientation error. + + The reward is the inverse of the orientation error between the object orientation and the goal orientation. + + Args: + env: The environment object. + command_name: The command term to be used for extracting the goal. + object_cfg: The configuration for the scene entity. Default is "object". + rot_eps: The threshold for the orientation error. Default is 1e-3. + """ + # extract useful elements + asset: RigidObject = env.scene[object_cfg.name] + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + + # obtain the goal orientation + goal_quat_w = command_term.command[:, 3:7] + # calculate the orientation error + dtheta = math_utils.quat_error_magnitude(asset.data.root_quat_w, goal_quat_w) + + return 1.0 / (dtheta + rot_eps) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py new file mode 100644 index 00000000000..8697861fe4b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/inhand/mdp/terminations.py @@ -0,0 +1,88 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Functions specific to the in-hand dexterous manipulation environments.""" + +import torch +from typing import TYPE_CHECKING + +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from .commands import InHandReOrientationCommand + + +def max_consecutive_success(env: ManagerBasedRLEnv, num_success: int, command_name: str) -> torch.Tensor: + """Check if the task has been completed consecutively for a certain number of times. + + Args: + env: The environment object. + num_success: Threshold for the number of consecutive successes required. + command_name: The command term to be used for extracting the goal. + """ + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + + return command_term.metrics["consecutive_success"] >= num_success + + +def object_away_from_goal( + env: ManagerBasedRLEnv, + threshold: float, + command_name: str, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Check if object has gone far from the goal. + + The object is considered to be out-of-reach if the distance between the goal and the object is greater + than the threshold. + + Args: + env: The environment object. + threshold: The threshold for the distance between the robot and the object. + command_name: The command term to be used for extracting the goal. + object_cfg: The configuration for the scene entity. Default is "object". + """ + # extract useful elements + command_term: InHandReOrientationCommand = env.command_manager.get_term(command_name) + asset = env.scene[object_cfg.name] + + # object pos + asset_pos_e = asset.data.root_pos_w - env.scene.env_origins + goal_pos_e = command_term.command[:, :3] + + return torch.norm(asset_pos_e - goal_pos_e, p=2, dim=1) > threshold + + +def object_away_from_robot( + env: ManagerBasedRLEnv, + threshold: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Check if object has gone far from the robot. + + The object is considered to be out-of-reach if the distance between the robot and the object is greater + than the threshold. + + Args: + env: The environment object. + threshold: The threshold for the distance between the robot and the object. + asset_cfg: The configuration for the robot entity. Default is "robot". + object_cfg: The configuration for the object entity. Default is "object". + """ + # extract useful elements + robot = env.scene[asset_cfg.name] + object = env.scene[object_cfg.name] + + # compute distance + dist = torch.norm(robot.data.root_pos_w - object.data.root_pos_w, dim=1) + + return dist > threshold diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/__init__.py new file mode 100644 index 00000000000..b6e4f355449 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object lift environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py new file mode 100644 index 00000000000..b6e4f355449 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object lift environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py new file mode 100644 index 00000000000..3349c2a79d1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/__init__.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import gymnasium as gym +import os + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Lift-Cube-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Lift-Cube-Franka-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Absolute Pose Control +## + +gym.register( + id="Isaac-Lift-Cube-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:FrankaCubeLiftEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Lift-Teddy-Bear-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:FrankaTeddyBearLiftEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Lift-Cube-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_rel_env_cfg:FrankaCubeLiftEnvCfg", + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc.json"), + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..6ed68d88912 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,79 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: franka_lift + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: False + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 100000000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.001 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + clip_actions: False + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bc.json b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bc.json new file mode 100644 index 00000000000..e96f7f7e194 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bc.json @@ -0,0 +1,264 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bc_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": "train", + "hdf5_validation_filter_key": "valid", + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "policy": { + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [ + 1024, + 1024 + ], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": false, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "joint_pos", + "joint_vel", + "object_position", + "target_object_position" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bcq.json b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bcq.json new file mode 100644 index 00000000000..1d80b50d287 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/robomimic/bcq.json @@ -0,0 +1,299 @@ +{ + "algo_name": "bcq", + "experiment": { + "name": "bcq", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": true, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": false + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bcq_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "critic": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "action_sampler": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "actor": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + } + }, + "discount": 0.99, + "n_step": 1, + "target_tau": 0.005, + "infinite_horizon": false, + "critic": { + "use_huber": false, + "max_gradient_norm": null, + "value_bounds": null, + "num_action_samples": 10, + "num_action_samples_rollout": 100, + "ensemble": { + "n": 2, + "weight": 0.75 + }, + "distributional": { + "enabled": false, + "num_atoms": 51 + }, + "layer_dims": [ + 300, + 400 + ] + }, + "action_sampler": { + "actor_layer_dims": [ + 1024, + 1024 + ], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": true, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "freeze_encoder_epoch": -1 + }, + "actor": { + "enabled": false, + "perturbation_scale": 0.05, + "layer_dims": [ + 300, + 400 + ] + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "tool_dof_pos_scaled", + "tool_positions", + "object_relative_tool_positions", + "object_desired_positions" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..99e67b784a9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class LiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "franka_lift" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.006, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-4, + schedule="adaptive", + gamma=0.98, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml new file mode 100644 index 00000000000..6d6f15781ac --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,28 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 42 + +# epoch * n_steps * nenvs: 500×512*8*8 +n_timesteps: 16384000 +policy: 'MlpPolicy' +n_steps: 64 +# mini batch size: num_envs * nsteps / nminibatches 2048×512÷2048 +batch_size: 192 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 8 +ent_coef: 0.00 +vf_coef: 0.0001 +learning_rate: !!float 3e-4 +clip_range: 0.2 +policy_kwargs: "dict( + activation_fn=nn.ELU, + net_arch=dict(pi=[256, 128, 64], vf=[256, 128, 64]) + )" +target_kl: 0.01 +max_grad_norm: 1.0 + +# # Uses VecNormalize class to normalize obs +# normalize_input: True +# # Uses VecNormalize class to normalize rew +# normalize_value: True +# clip_obs: 5 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..94337d46bac --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 8 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.001 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "franka_lift" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py new file mode 100644 index 00000000000..57062bd958f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_abs_env_cfg.py @@ -0,0 +1,115 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.assets import DeformableObjectCfg +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sim.spawners import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +import isaaclab_tasks.manager_based.manipulation.lift.mdp as mdp + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +## +# Rigid object lift environment. +## + + +@configclass +class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + + +## +# Deformable object lift environment. +## + + +@configclass +class FrankaTeddyBearLiftEnvCfg(FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.object = DeformableObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.5, 0, 0.05), rot=(0.707, 0, 0, 0.707)), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Objects/Teddy_Bear/teddy_bear.usd", + scale=(0.01, 0.01, 0.01), + ), + ) + + # Make the end effector less stiff to not hurt the poor teddy bear + self.scene.robot.actuators["panda_hand"].effort_limit_sim = 50.0 + self.scene.robot.actuators["panda_hand"].stiffness = 40.0 + self.scene.robot.actuators["panda_hand"].damping = 10.0 + + # Disable replicate physics as it doesn't work for deformable objects + # FIXME: This should be fixed by the PhysX replication system. + self.scene.replicate_physics = False + + # Set events for the specific object type (deformable cube) + self.events.reset_object_position = EventTerm( + func=mdp.reset_nodal_state_uniform, + mode="reset", + params={ + "position_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + # Remove all the terms for the state machine demo + # TODO: Computing the root pose of deformable object from nodal positions is expensive. + # We need to fix that part before enabling these terms for the training. + self.terminations.object_dropping = None + self.rewards.reaching_object = None + self.rewards.lifting_object = None + self.rewards.object_goal_tracking = None + self.rewards.object_goal_tracking_fine_grained = None + self.observations.policy.object_position = None diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py new file mode 100644 index 00000000000..1af53043e99 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/ik_rel_env_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py new file mode 100644 index 00000000000..c0de6b3bd3b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/config/franka/joint_pos_env_cfg.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.assets import RigidObjectCfg +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.lift import mdp +from isaaclab_tasks.manager_based.manipulation.lift.lift_env_cfg import LiftEnvCfg + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaCubeLiftEnvCfg(LiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + # Set the body name for the end effector + self.commands.object_pose.body_name = "panda_hand" + + # Set Cube as object + self.scene.object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.055], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(0.8, 0.8, 0.8), + rigid_props=RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ), + ), + ) + + # Listens to the required transforms + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.1034], + ), + ), + ], + ) + + +@configclass +class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py new file mode 100644 index 00000000000..7b6495cc1a0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/lift_env_cfg.py @@ -0,0 +1,227 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, DeformableObjectCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +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.frame_transformer.frame_transformer_cfg import FrameTransformerCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + +## +# Scene definition +## + + +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the lift scene with a robot and a object. + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the target object, robot and end-effector frames + """ + + # robots: will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # end-effector sensor: will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + # target object: will be populated by agent env cfg + object: RigidObjectCfg | DeformableObjectCfg = MISSING + + # Table + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), + spawn=GroundPlaneCfg(), + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + object_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name=MISSING, # will be set by agent env cfg + resampling_time_range=(5.0, 5.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.4, 0.6), pos_y=(-0.25, 0.25), pos_z=(0.25, 0.5), roll=(0.0, 0.0), pitch=(0.0, 0.0), yaw=(0.0, 0.0) + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg | mdp.DifferentialInverseKinematicsActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object_position = ObsTerm(func=mdp.object_position_in_robot_root_frame) + target_object_position = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_object_position = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object", body_names="Object"), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + reaching_object = RewTerm(func=mdp.object_ee_distance, params={"std": 0.1}, weight=1.0) + + lifting_object = RewTerm(func=mdp.object_is_lifted, params={"minimal_height": 0.04}, weight=15.0) + + object_goal_tracking = RewTerm( + func=mdp.object_goal_distance, + params={"std": 0.3, "minimal_height": 0.04, "command_name": "object_pose"}, + weight=16.0, + ) + + object_goal_tracking_fine_grained = RewTerm( + func=mdp.object_goal_distance, + params={"std": 0.05, "minimal_height": 0.04, "command_name": "object_pose"}, + weight=5.0, + ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-4) + + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-1e-4, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("object")} + ) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-1, "num_steps": 10000} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "joint_vel", "weight": -1e-1, "num_steps": 10000} + ) + + +## +# Environment configuration +## + + +@configclass +class LiftEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the lifting environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.episode_length_s = 5.0 + # simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = self.decimation + + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py new file mode 100644 index 00000000000..52f89b03e2c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the lift environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py new file mode 100644 index 00000000000..85f2a2ce334 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/observations.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import subtract_frame_transforms + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_position_in_robot_root_frame( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """The position of the object in the robot's root frame.""" + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + object_pos_w = object.data.root_pos_w[:, :3] + object_pos_b, _ = subtract_frame_transforms(robot.data.root_pos_w, robot.data.root_quat_w, object_pos_w) + return object_pos_b diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py new file mode 100644 index 00000000000..69b6f604f40 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/rewards.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformer +from isaaclab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_is_lifted( + env: ManagerBasedRLEnv, minimal_height: float, object_cfg: SceneEntityCfg = SceneEntityCfg("object") +) -> torch.Tensor: + """Reward the agent for lifting the object above the minimal height.""" + object: RigidObject = env.scene[object_cfg.name] + return torch.where(object.data.root_pos_w[:, 2] > minimal_height, 1.0, 0.0) + + +def object_ee_distance( + env: ManagerBasedRLEnv, + std: float, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +) -> torch.Tensor: + """Reward the agent for reaching the object using tanh-kernel.""" + # extract the used quantities (to enable type-hinting) + object: RigidObject = env.scene[object_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + # Target object position: (num_envs, 3) + cube_pos_w = object.data.root_pos_w + # End-effector position: (num_envs, 3) + ee_w = ee_frame.data.target_pos_w[..., 0, :] + # Distance of the end-effector to the object: (num_envs,) + object_ee_distance = torch.norm(cube_pos_w - ee_w, dim=1) + + return 1 - torch.tanh(object_ee_distance / std) + + +def object_goal_distance( + env: ManagerBasedRLEnv, + std: float, + minimal_height: float, + command_name: str, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Reward the agent for tracking the goal pose using tanh-kernel.""" + # extract the used quantities (to enable type-hinting) + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + command = env.command_manager.get_command(command_name) + # compute the desired position in the world frame + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms(robot.data.root_pos_w, robot.data.root_quat_w, des_pos_b) + # distance of the end-effector to the object: (num_envs,) + distance = torch.norm(des_pos_w - object.data.root_pos_w, dim=1) + # rewarded if the object is lifted above the threshold + return (object.data.root_pos_w[:, 2] > minimal_height) * (1 - torch.tanh(distance / std)) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py new file mode 100644 index 00000000000..c5d2858838a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/lift/mdp/terminations.py @@ -0,0 +1,58 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the lift task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_reached_goal( + env: ManagerBasedRLEnv, + command_name: str = "object_pose", + threshold: float = 0.02, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Termination condition for the object reaching the goal position. + + Args: + env: The environment. + command_name: The name of the command that is used to control the object. + threshold: The threshold for the object to reach the goal position. Defaults to 0.02. + robot_cfg: The robot configuration. Defaults to SceneEntityCfg("robot"). + object_cfg: The object configuration. Defaults to SceneEntityCfg("object"). + + """ + # extract the used quantities (to enable type-hinting) + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + command = env.command_manager.get_command(command_name) + # compute the desired position in the world frame + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms(robot.data.root_pos_w, robot.data.root_quat_w, des_pos_b) + # distance of the end-effector to the object: (num_envs,) + distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) + + # rewarded if the object is lifted above the threshold + return distance < threshold diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py new file mode 100644 index 00000000000..55d8f38276c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym +import os + +from . import agents, pickplace_gr1t2_env_cfg + +gym.register( + id="Isaac-PickPlace-GR1T2-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": pickplace_gr1t2_env_cfg.PickPlaceGR1T2EnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 00000000000..d2e0f8fc6d9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json @@ -0,0 +1,117 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_low_dim_gr1t2", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 100, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "dataset_keys": [ + "actions" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 2000, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state", + "object" + ], + "rgb": [], + "depth": [], + "scan": [] + } + } + } +} diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py new file mode 100644 index 00000000000..266c48c467b --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the lift environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py new file mode 100644 index 00000000000..917d4b5cd05 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_obs( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + """ + Object observations (in world frame): + object pos, + object quat, + left_eef to object, + right_eef_to object, + """ + + body_pos_w = env.scene["robot"].data.body_pos_w + left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins + right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins + + object_pos = env.scene["object"].data.root_pos_w - env.scene.env_origins + object_quat = env.scene["object"].data.root_quat_w + + left_eef_to_object = object_pos - left_eef_pos + right_eef_to_object = object_pos - right_eef_pos + + return torch.cat( + ( + object_pos, + object_quat, + left_eef_to_object, + right_eef_to_object, + ), + dim=1, + ) + + +def get_left_eef_pos( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_pos_w = env.scene["robot"].data.body_pos_w + left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins + + return left_eef_pos + + +def get_left_eef_quat( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_quat_w = env.scene["robot"].data.body_quat_w + left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_quat = body_quat_w[:, left_eef_idx] + + return left_eef_quat + + +def get_right_eef_pos( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_pos_w = env.scene["robot"].data.body_pos_w + right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins + + return right_eef_pos + + +def get_right_eef_quat( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_quat_w = env.scene["robot"].data.body_quat_w + right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + right_eef_quat = body_quat_w[:, right_eef_idx] + + return right_eef_quat + + +def get_hand_state( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + hand_joint_states = env.scene["robot"].data.joint_pos[:, -22:] # Hand joints are last 22 entries of joint state + + return hand_joint_states + + +def get_head_state( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + robot_joint_names = env.scene["robot"].data.joint_names + head_joint_names = ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"] + indexes = torch.tensor([robot_joint_names.index(name) for name in head_joint_names], dtype=torch.long) + head_joint_states = env.scene["robot"].data.joint_pos[:, indexes] + + return head_joint_states + + +def get_all_robot_link_state( + env: ManagerBasedRLEnv, +) -> torch.Tensor: + body_pos_w = env.scene["robot"].data.body_link_state_w[:, :, :] + all_robot_link_pos = body_pos_w + + return all_robot_link_pos diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py new file mode 100644 index 00000000000..a6b2b10a0a8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the lift task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def task_done( + env: ManagerBasedRLEnv, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + right_wrist_max_x: float = 0.26, + min_x: float = 0.30, + max_x: float = 0.95, + min_y: float = 0.25, + max_y: float = 0.66, + min_height: float = 1.13, + min_vel: float = 0.20, +) -> torch.Tensor: + """Determine if the object placement task is complete. + + This function checks whether all success conditions for the task have been met: + 1. object is within the target x/y range + 2. object is below a minimum height + 3. object velocity is below threshold + 4. Right robot wrist is retracted back towards body (past a given x pos threshold) + + Args: + env: The RL environment instance. + object_cfg: Configuration for the object entity. + right_wrist_max_x: Maximum x position of the right wrist for task completion. + min_x: Minimum x position of the object for task completion. + max_x: Maximum x position of the object for task completion. + min_y: Minimum y position of the object for task completion. + max_y: Maximum y position of the object for task completion. + min_height: Minimum height (z position) of the object for task completion. + min_vel: Minimum velocity magnitude of the object for task completion. + + Returns: + Boolean tensor indicating which environments have completed the task. + """ + # Get object entity from the scene + object: RigidObject = env.scene[object_cfg.name] + + # Extract wheel position relative to environment origin + wheel_x = object.data.root_pos_w[:, 0] - env.scene.env_origins[:, 0] + wheel_y = object.data.root_pos_w[:, 1] - env.scene.env_origins[:, 1] + wheel_height = object.data.root_pos_w[:, 2] - env.scene.env_origins[:, 2] + wheel_vel = torch.abs(object.data.root_vel_w) + + # Get right wrist position relative to environment origin + robot_body_pos_w = env.scene["robot"].data.body_pos_w + right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + right_wrist_x = robot_body_pos_w[:, right_eef_idx, 0] - env.scene.env_origins[:, 0] + + # Check all success conditions and combine with logical AND + done = wheel_x < max_x + done = torch.logical_and(done, wheel_x > min_x) + done = torch.logical_and(done, wheel_y < max_y) + done = torch.logical_and(done, wheel_y > min_y) + done = torch.logical_and(done, wheel_height < min_height) + done = torch.logical_and(done, right_wrist_x < right_wrist_max_x) + done = torch.logical_and(done, wheel_vel[:, 0] < min_vel) + done = torch.logical_and(done, wheel_vel[:, 1] < min_vel) + done = torch.logical_and(done, wheel_vel[:, 2] < min_vel) + + return done diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py new file mode 100644 index 00000000000..a202cd22133 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -0,0 +1,412 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import tempfile +import torch + +from pink.tasks import FrameTask + +import isaaclab.controllers.utils as ControllerUtils +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +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 SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + # Object + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.40, 1.0413], rot=[1, 0, 0, 0]), + spawn=sim_utils.CylinderCfg( + radius=0.018, + height=0.35, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.3), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.15, 0.15, 0.15), metallic=1.0), + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="max", + restitution_combine_mode="min", + static_friction=0.9, + dynamic_friction=0.9, + restitution=0.0, + ), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = GR1T2_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.93), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_pitch_joint": -1.5708, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_pitch_joint": -1.5708, + "left_wrist_yaw_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + # -- + "head_.*": 0.0, + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + "R_.*": 0.0, + "L_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + pink_ik_cfg = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "left_wrist_yaw_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "right_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + ], + # Joints to be locked in URDF + ik_urdf_fixed_joint_names=[ + "left_hip_roll_joint", + "right_hip_roll_joint", + "left_hip_yaw_joint", + "right_hip_yaw_joint", + "left_hip_pitch_joint", + "right_hip_pitch_joint", + "left_knee_pitch_joint", + "right_knee_pitch_joint", + "left_ankle_pitch_joint", + "right_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_ankle_roll_joint", + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + "head_roll_joint", + "head_pitch_joint", + "head_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + hand_joint_names=[ + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=22, + show_ik_warnings=False, + variable_input_tasks=[ + FrameTask( + "GR1T2_fourier_hand_6dof_left_hand_pitch_link", + position_cost=1.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.1, + ), + FrameTask( + "GR1T2_fourier_hand_6dof_right_hand_pitch_link", + position_cost=1.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.1, + ), + ], + fixed_input_tasks=[ + # COMMENT OUT IF LOCKING WAIST/HEAD + # FrameTask( + # "GR1T2_fourier_hand_6dof_head_yaw_link", + # position_cost=1.0, # [cost] / [m] + # orientation_cost=0.05, # [cost] / [rad] + # ), + ], + ), + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) + left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) + right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) + right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) + + hand_joint_state = ObsTerm(func=mdp.get_hand_state) + head_joint_state = ObsTerm(func=mdp.get_head_state) + + object = ObsTerm(func=mdp.object_obs) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=mdp.task_done) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_object = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.05, 0.0], + "y": [0.0, 0.05], + }, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + +@configclass +class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), + # left hand joint pos (11), right hand joint pos (11)] + idle_action = torch.tensor([ + -0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.22878, + 0.2536, + 1.0953, + 0.5, + 0.5, + -0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ]) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 5 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 60 # 100Hz + self.sim.render_interval = 2 + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + ControllerUtils.change_revolute_to_fixed( + temp_urdf_output_path, self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path + self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/__init__.py new file mode 100644 index 00000000000..04a3c6f8993 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Fixed-arm environments with end-effector pose tracking commands.""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py new file mode 100644 index 00000000000..650a7af9182 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for arm-based reach-tracking environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py new file mode 100644 index 00000000000..eb29d2f15f7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/__init__.py @@ -0,0 +1,96 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Reach-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaReachEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Reach-Franka-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaReachEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + + +## +# Inverse Kinematics - Absolute Pose Control +## + +gym.register( + id="Isaac-Reach-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:FrankaReachEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Reach-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_rel_env_cfg:FrankaReachEnvCfg", + }, + disable_env_checker=True, +) + +## +# Operational Space Control +## + +gym.register( + id="Isaac-Reach-Franka-OSC-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaReachEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Reach-Franka-OSC-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaReachEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..b568d40f26a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [64, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: reach_franka + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 10000 + max_epochs: 1000 + save_best_after: 200 + save_frequency: 100 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.01 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2 + clip_value: True + clip_actions: False + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..b84845445a3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class FrankaReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1000 + save_interval = 50 + experiment_name = "franka_reach" + run_name = "" + resume = False + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.001, + num_learning_epochs=8, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..ef0232ccace --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "reach_franka" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 24000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py new file mode 100644 index 00000000000..164ac0b0464 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaReachEnvCfg(joint_pos_env_cfg.FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py new file mode 100644 index 00000000000..5589852b539 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaReachEnvCfg(joint_pos_env_cfg.FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py new file mode 100644 index 00000000000..f379c54a607 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab_tasks.manager_based.manipulation.reach.reach_env_cfg import ReachEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets import FRANKA_PANDA_CFG # isort: skip + + +## +# Environment configuration +## + + +@configclass +class FrankaReachEnvCfg(ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to franka + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # override rewards + self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["panda_hand"] + self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = ["panda_hand"] + self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["panda_hand"] + + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True + ) + # override command generator body + # end-effector is along z-direction + self.commands.ee_pose.body_name = "panda_hand" + self.commands.ee_pose.ranges.pitch = (math.pi, math.pi) + + +@configclass +class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py new file mode 100644 index 00000000000..b358da19f7f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.operational_space_cfg import OperationalSpaceControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaReachEnvCfg(joint_pos_env_cfg.FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We remove stiffness and damping for the shoulder and forearm joints for effort control + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["panda_shoulder"].stiffness = 0.0 + self.scene.robot.actuators["panda_shoulder"].damping = 0.0 + self.scene.robot.actuators["panda_forearm"].stiffness = 0.0 + self.scene.robot.actuators["panda_forearm"].damping = 0.0 + self.scene.robot.spawn.rigid_props.disable_gravity = True + + # If closed-loop contact force control is desired, contact sensors should be enabled for the robot + # self.scene.robot.spawn.activate_contact_sensors = True + + self.actions.arm_action = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + # If a task frame different from articulation root/base is desired, a RigidObject, e.g., "task_frame", + # can be added to the scene and its relative path could provided as task_frame_rel_path + # task_frame_rel_path="task_frame", + controller_cfg=OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=100.0, + motion_damping_ratio_task=1.0, + motion_stiffness_limits_task=(50.0, 200.0), + nullspace_control="position", + ), + nullspace_joint_pos_target="center", + position_scale=1.0, + orientation_scale=1.0, + stiffness_scale=100.0, + ) + # Removing these observations as they are not needed for OSC and we want keep the observation space small + self.observations.policy.joint_pos = None + self.observations.policy.joint_vel = None + + +@configclass +class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 16 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py new file mode 100644 index 00000000000..acfdec073a0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/__init__.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Reach-UR10-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10ReachEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10ReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Reach-UR10-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR10ReachEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:UR10ReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..ab0b67fc7f4 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,78 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [64, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: reach_ur10 + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: True + num_actors: -1 + reward_shaper: + scale_value: 1.0 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 10000 + max_epochs: 1000 + save_best_after: 200 + save_frequency: 100 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.01 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 5 + critic_coef: 2 + clip_value: True + clip_actions: False + bounds_loss_coef: 0.0001 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..f7d8588589d --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class UR10ReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1000 + save_interval = 50 + experiment_name = "reach_ur10" + run_name = "" + resume = False + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=8, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..5ffaffd1175 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [64, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.01 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "reach_ur10" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 24000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py new file mode 100644 index 00000000000..10e619f49a8 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py @@ -0,0 +1,62 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab_tasks.manager_based.manipulation.reach.reach_env_cfg import ReachEnvCfg + +## +# Pre-defined configs +## +from isaaclab_assets import UR10_CFG # isort: skip + + +## +# Environment configuration +## + + +@configclass +class UR10ReachEnvCfg(ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # switch robot to ur10 + self.scene.robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # override events + self.events.reset_robot_joints.params["position_range"] = (0.75, 1.25) + # override rewards + self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["ee_link"] + self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = ["ee_link"] + self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["ee_link"] + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True + ) + # override command generator body + # end-effector is along x-direction + self.commands.ee_pose.body_name = "ee_link" + self.commands.ee_pose.ranges.pitch = (math.pi / 2, math.pi / 2) + + +@configclass +class UR10ReachEnvCfg_PLAY(UR10ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py new file mode 100644 index 00000000000..46401f45faf --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .rewards import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py new file mode 100644 index 00000000000..029650bc8e3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/mdp/rewards.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms, quat_error_magnitude, quat_mul + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def position_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking of the position error using L2-norm. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame). The position error is computed as the L2-norm + of the difference between the desired and current positions. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms(asset.data.root_pos_w, asset.data.root_quat_w, des_pos_b) + curr_pos_w = asset.data.body_pos_w[:, asset_cfg.body_ids[0]] # type: ignore + return torch.norm(curr_pos_w - des_pos_w, dim=1) + + +def position_command_error_tanh( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward tracking of the position using the tanh kernel. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame) and maps it with a tanh kernel. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms(asset.data.root_pos_w, asset.data.root_quat_w, des_pos_b) + curr_pos_w = asset.data.body_pos_w[:, asset_cfg.body_ids[0]] # type: ignore + distance = torch.norm(curr_pos_w - des_pos_w, dim=1) + return 1 - torch.tanh(distance / std) + + +def orientation_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking orientation error using shortest path. + + The function computes the orientation error between the desired orientation (from the command) and the + current orientation of the asset's body (in world frame). The orientation error is computed as the shortest + path between the desired and current orientations. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current orientations + des_quat_b = command[:, 3:7] + des_quat_w = quat_mul(asset.data.root_quat_w, des_quat_b) + curr_quat_w = asset.data.body_quat_w[:, asset_cfg.body_ids[0]] # type: ignore + return quat_error_magnitude(curr_quat_w, des_quat_w) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py new file mode 100644 index 00000000000..df9d3c15a55 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -0,0 +1,213 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import CurriculumTermCfg as CurrTerm +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.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp + +## +# Scene definition +## + + +@configclass +class ReachSceneCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # world + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.55, 0.0, 0.0), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + # robots + robot: ArticulationCfg = MISSING + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + ee_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name=MISSING, + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.35, 0.65), + pos_y=(-0.2, 0.2), + pos_z=(0.15, 0.5), + roll=(0.0, 0.0), + pitch=MISSING, # depends on end-effector axis + yaw=(-3.14, 3.14), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action: ActionTerm = MISSING + gripper_action: ActionTerm | None = None + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose"}) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (0.5, 1.5), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # task terms + end_effector_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", body_names=MISSING), "command_name": "ee_pose"}, + ) + end_effector_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg("robot", body_names=MISSING), "std": 0.1, "command_name": "ee_pose"}, + ) + end_effector_orientation_tracking = RewTerm( + func=mdp.orientation_command_error, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", body_names=MISSING), "command_name": "ee_pose"}, + ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.0001) + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -0.005, "num_steps": 4500} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "joint_vel", "weight": -0.001, "num_steps": 4500} + ) + + +## +# Environment configuration +## + + +@configclass +class ReachEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the reach end-effector pose tracking environment.""" + + # Scene settings + scene: ReachSceneCfg = ReachSceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.sim.render_interval = self.decimation + self.episode_length_s = 12.0 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.sim.dt = 1.0 / 60.0 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/__init__.py new file mode 100644 index 00000000000..d4605721edd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object stack environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py new file mode 100644 index 00000000000..d4605721edd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object stack environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py new file mode 100644 index 00000000000..dc4d8dd85b6 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/__init__.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import gymnasium as gym +import os + +from . import ( + agents, + stack_ik_abs_env_cfg, + stack_ik_rel_blueprint_env_cfg, + stack_ik_rel_env_cfg, + stack_ik_rel_instance_randomize_env_cfg, + stack_ik_rel_visuomotor_env_cfg, + stack_joint_pos_env_cfg, + stack_joint_pos_instance_randomize_env_cfg, +) + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Stack-Cube-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Instance-Randomize-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_joint_pos_instance_randomize_env_cfg.FrankaCubeStackInstanceRandomizeEnvCfg, + }, + disable_env_checker=True, +) + + +## +# Inverse Kinematics - Relative Pose Control +## + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_ik_rel_env_cfg.FrankaCubeStackEnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_ik_rel_visuomotor_env_cfg.FrankaCubeStackVisuomotorEnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_image_84.json"), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_ik_abs_env_cfg.FrankaCubeStackEnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Instance-Randomize-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_ik_rel_instance_randomize_env_cfg.FrankaCubeStackInstanceRandomizeEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_ik_rel_blueprint_env_cfg.FrankaCubeStackBlueprintEnvCfg, + }, + disable_env_checker=True, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_84.json b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_84.json new file mode 100644 index 00000000000..94e722fd0b1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_image_84.json @@ -0,0 +1,219 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_image_franka_stack", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 20, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 500, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "low_dim", + "hdf5_use_swmr": true, + "hdf5_load_next_obs": false, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "pad_seq_length": true, + "frame_stack": 1, + "pad_frame_stack": true, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 16, + "num_epochs": 600, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": true, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 1000, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "eef_pos", + "eef_quat", + "gripper_pos" + ], + "rgb": [ + "table_cam", + "wrist_cam" + ], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": "CropRandomizer", + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + } + } + } +} diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 00000000000..23e490971f3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json @@ -0,0 +1,101 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_low_dim_franka_stack", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 100, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "dataset_keys": [ + "actions" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 2000, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gmm": { + "enabled": true, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "eef_pos", + "eef_quat", + "gripper_pos", + "object" + ], + "rgb": [], + "depth": [], + "scan": [] + } + } + } +} diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py new file mode 100644 index 00000000000..208baebdbcf --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py new file mode 100644 index 00000000000..d3d914bada7 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py @@ -0,0 +1,278 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os +import torch +from torchvision.utils import save_image + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs import ManagerBasedEnv +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import Camera, CameraCfg, RayCasterCamera, TiledCamera +from isaaclab.utils import configclass + +from ... import mdp +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +def image( + env: ManagerBasedEnv, + sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"), + data_type: str = "rgb", + convert_perspective_to_orthogonal: bool = False, + normalize: bool = True, + save_image_to_file: bool = False, + image_path: str = "image", +) -> torch.Tensor: + """Images of a specific datatype from the camera sensor. + + If the flag :attr:`normalize` is True, post-processing of the images are performed based on their + data-types: + + - "rgb": Scales the image to (0, 1) and subtracts with the mean of the current image batch. + - "depth" or "distance_to_camera" or "distance_to_plane": Replaces infinity values with zero. + + Args: + env: The environment the cameras are placed within. + sensor_cfg: The desired sensor to read from. Defaults to SceneEntityCfg("tiled_camera"). + data_type: The data type to pull from the desired camera. Defaults to "rgb". + convert_perspective_to_orthogonal: Whether to orthogonalize perspective depth images. + This is used only when the data type is "distance_to_camera". Defaults to False. + normalize: Whether to normalize the images. This depends on the selected data type. + Defaults to True. + + Returns: + The images produced at the last time-step + """ + # extract the used quantities (to enable type-hinting) + sensor: TiledCamera | Camera | RayCasterCamera = env.scene.sensors[sensor_cfg.name] + + # obtain the input image + images = sensor.data.output[data_type] + + # depth image conversion + if (data_type == "distance_to_camera") and convert_perspective_to_orthogonal: + images = math_utils.orthogonalize_perspective_depth(images, sensor.data.intrinsic_matrices) + + # rgb/depth image normalization + if normalize: + if data_type == "rgb": + images = images.float() / 255.0 + mean_tensor = torch.mean(images, dim=(1, 2), keepdim=True) + images -= mean_tensor + elif "distance_to" in data_type or "depth" in data_type: + images[images == float("inf")] = 0 + elif data_type == "normals": + images = (images + 1.0) * 0.5 + + if save_image_to_file: + dir_path, _ = os.path.split(image_path) + if dir_path: + os.makedirs(dir_path, exist_ok=True) + if images.dtype == torch.uint8: + images = images.float() / 255.0 + # Get total successful episodes + total_successes = 0 + if hasattr(env, "recorder_manager") and env.recorder_manager is not None: + total_successes = env.recorder_manager.exported_successful_episode_count + + for tile in range(images.shape[0]): + tile_chw = torch.swapaxes(images[tile : tile + 1].unsqueeze(1), 1, -1).squeeze(-1) + filename = ( + f"{image_path}_{data_type}_trial_{total_successes}_tile_{tile}_step_{env.common_step_counter}.png" + ) + save_image(tile_chw, filename) + + return images.clone() + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object = ObsTerm(func=mdp.object_obs) + cube_positions = ObsTerm(func=mdp.cube_positions_in_world_frame) + cube_orientations = ObsTerm(func=mdp.cube_orientations_in_world_frame) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class RGBCameraPolicyCfg(ObsGroup): + """Observations for policy group with RGB images.""" + + table_cam_normals = ObsTerm( + func=image, + params={ + "sensor_cfg": SceneEntityCfg("table_cam"), + "data_type": "normals", + "normalize": True, + "save_image_to_file": True, + "image_path": "table_cam", + }, + ) + table_cam_segmentation = ObsTerm( + func=image, + params={ + "sensor_cfg": SceneEntityCfg("table_cam"), + "data_type": "semantic_segmentation", + "normalize": False, + "save_image_to_file": True, + "image_path": "table_cam", + }, + ) + table_high_cam_normals = ObsTerm( + func=image, + params={ + "sensor_cfg": SceneEntityCfg("table_high_cam"), + "data_type": "normals", + "normalize": True, + "save_image_to_file": True, + "image_path": "table_high_cam", + }, + ) + table_high_cam_segmentation = ObsTerm( + func=image, + params={ + "sensor_cfg": SceneEntityCfg("table_high_cam"), + "data_type": "semantic_segmentation", + "normalize": False, + "save_image_to_file": True, + "image_path": "table_high_cam", + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp_1 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_2"), + }, + ) + stack_1 = ObsTerm( + func=mdp.object_stacked, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "upper_object_cfg": SceneEntityCfg("cube_2"), + "lower_object_cfg": SceneEntityCfg("cube_1"), + }, + ) + grasp_2 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_3"), + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + rgb_camera: RGBCameraPolicyCfg = RGBCameraPolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class FrankaCubeStackBlueprintEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + observations: ObservationsCfg = ObservationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.spawn.semantic_tags = [("class", "robot")] + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + MAPPING = { + "class:cube_1": (255, 36, 66, 255), + "class:cube_2": (255, 184, 48, 255), + "class:cube_3": (55, 255, 139, 255), + "class:table": (255, 237, 218, 255), + "class:ground": (100, 100, 100, 255), + "class:robot": (125, 125, 125, 255), + "class:UNLABELLED": (125, 125, 125, 255), + "class:BACKGROUND": (10, 10, 10, 255), + } + + # Set table view camera + self.scene.table_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/table_cam", + update_period=0.0333, + height=704, + width=1280, + data_types=["rgb", "semantic_segmentation", "normals"], + colorize_semantic_segmentation=True, + semantic_segmentation_mapping=MAPPING, + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(1.0, 0.0, 0.33), rot=(-0.3799, 0.5963, 0.5963, -0.3799), convention="ros"), + ) + + # Set table view camera + self.scene.table_high_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/table_high_cam", + update_period=0.0333, + height=704, + width=1280, + data_types=["rgb", "semantic_segmentation", "normals"], + colorize_semantic_segmentation=True, + semantic_segmentation_mapping=MAPPING, + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(1.5, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(1.4, 1.8, 1.2), rot=(-0.1393, 0.2025, 0.8185, -0.5192), convention="ros"), + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py new file mode 100644 index 00000000000..2f8a81f2a4f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py new file mode 100644 index 00000000000..9067c51da8a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_instance_randomize_env_cfg.py @@ -0,0 +1,46 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import stack_joint_pos_instance_randomize_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaCubeStackInstanceRandomizeEnvCfg( + stack_joint_pos_instance_randomize_env_cfg.FrankaCubeStackInstanceRandomizeEnvCfg +): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Reduce the number of environments due to camera resources + self.scene.num_envs = 2 + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py new file mode 100644 index 00000000000..dc04aa351f9 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_env_cfg.py @@ -0,0 +1,153 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import CameraCfg +from isaaclab.utils import configclass + +from ... import mdp +from . import stack_joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object = ObsTerm(func=mdp.object_obs) + cube_positions = ObsTerm(func=mdp.cube_positions_in_world_frame) + cube_orientations = ObsTerm(func=mdp.cube_orientations_in_world_frame) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + table_cam = ObsTerm( + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False} + ) + wrist_cam = ObsTerm( + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("wrist_cam"), "data_type": "rgb", "normalize": False} + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp_1 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_2"), + }, + ) + stack_1 = ObsTerm( + func=mdp.object_stacked, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "upper_object_cfg": SceneEntityCfg("cube_2"), + "lower_object_cfg": SceneEntityCfg("cube_1"), + }, + ) + grasp_2 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_3"), + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class FrankaCubeStackVisuomotorEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg): + observations: ObservationsCfg = ObservationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + # Set cameras + # Set wrist camera + self.scene.wrist_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand/wrist_cam", + update_period=0.0, + height=84, + width=84, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 2) + ), + offset=CameraCfg.OffsetCfg( + pos=(0.13, 0.0, -0.15), rot=(-0.70614, 0.03701, 0.03701, -0.70614), convention="ros" + ), + ) + + # Set table view camera + self.scene.table_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/table_cam", + update_period=0.0, + height=84, + width=84, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 2) + ), + offset=CameraCfg.OffsetCfg( + pos=(1.0, 0.0, 0.4), rot=(0.35355, -0.61237, -0.61237, 0.35355), convention="ros" + ), + ) + + # Set settings for camera rendering + self.rerender_on_reset = True + self.sim.render.antialiasing_mode = "OFF" # disable dlss + + # List of image observations in policy observations + self.image_obs_list = ["table_cam", "wrist_cam"] diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py new file mode 100644 index 00000000000..b7a08080a0e --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py @@ -0,0 +1,168 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.assets import RigidObjectCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events +from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import StackEnvCfg + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class EventCfg: + """Configuration for events.""" + + init_franka_arm_pose = EventTerm( + func=franka_stack_events.set_default_joint_pose, + mode="startup", + params={ + "default_pose": [0.0444, -0.1894, -0.1107, -2.5148, 0.0044, 2.3775, 0.6952, 0.0400, 0.0400], + }, + ) + + randomize_franka_joint_state = EventTerm( + func=franka_stack_events.randomize_joint_by_gaussian_offset, + mode="reset", + params={ + "mean": 0.0, + "std": 0.02, + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + randomize_cube_positions = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": {"x": (0.4, 0.6), "y": (-0.10, 0.10), "z": (0.0203, 0.0203), "yaw": (-1.0, 1, 0)}, + "min_separation": 0.1, + "asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + }, + ) + + +@configclass +class FrankaCubeStackEnvCfg(StackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set events + self.events = EventCfg() + + # Set Franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.spawn.semantic_tags = [("class", "robot")] + + # Add semantics to table + self.scene.table.spawn.semantic_tags = [("class", "table")] + + # Add semantics to ground + self.scene.plane.semantic_tags = [("class", "ground")] + + # Set actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + + # Rigid body properties of each cube + cube_properties = RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + # Set each stacking cube deterministically + self.scene.cube_1 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_1", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_1")], + ), + ) + self.scene.cube_2 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.55, 0.05, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_2")], + ), + ) + self.scene.cube_3 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_3", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.60, -0.1, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_3")], + ), + ) + + # Listens to the required transforms + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.1034], + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + name="tool_rightfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_leftfinger", + name="tool_leftfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + ], + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py new file mode 100644 index 00000000000..783b555760c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py @@ -0,0 +1,232 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg, RigidObjectCollectionCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import CameraCfg, FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events +from isaaclab_tasks.manager_based.manipulation.stack.stack_instance_randomize_env_cfg import ( + StackInstanceRandomizeEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class EventCfg: + """Configuration for events.""" + + init_franka_arm_pose = EventTerm( + func=franka_stack_events.set_default_joint_pose, + mode="startup", + params={ + "default_pose": [0.0444, -0.1894, -0.1107, -2.5148, 0.0044, 2.3775, 0.6952, 0.0400, 0.0400], + }, + ) + + randomize_franka_joint_state = EventTerm( + func=franka_stack_events.randomize_joint_by_gaussian_offset, + mode="reset", + params={ + "mean": 0.0, + "std": 0.02, + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + randomize_cubes_in_focus = EventTerm( + func=franka_stack_events.randomize_rigid_objects_in_focus, + mode="reset", + params={ + "asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + "out_focus_state": torch.tensor([10.0, 10.0, 10.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + "pose_range": {"x": (0.4, 0.6), "y": (-0.10, 0.10), "z": (0.0203, 0.0203), "yaw": (-1.0, 1, 0)}, + "min_separation": 0.1, + }, + ) + + +@configclass +class FrankaCubeStackInstanceRandomizeEnvCfg(StackInstanceRandomizeEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set events + self.events = EventCfg() + + # Set Franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Reduce the number of environments due to camera resources + self.scene.num_envs = 2 + + # Set actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + + # Rigid body properties of each cube + cube_properties = RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + # Set each stacking cube to be a collection of rigid objects + cube_1_config_dict = { + "blue_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_1_Blue", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + "red_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_1_Red", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0403], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + } + + cube_2_config_dict = { + "red_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2_Red", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.55, 0.05, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + "yellow_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2_Yellow", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.55, 0.05, 0.0403], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/yellow_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + } + + cube_3_config_dict = { + "yellow_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_3_Yellow", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.60, -0.1, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/yellow_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + "green_cube": RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2_Green", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.60, -0.1, 0.0403], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=cube_properties, + ), + ), + } + + self.scene.cube_1 = RigidObjectCollectionCfg(rigid_objects=cube_1_config_dict) + self.scene.cube_2 = RigidObjectCollectionCfg(rigid_objects=cube_2_config_dict) + self.scene.cube_3 = RigidObjectCollectionCfg(rigid_objects=cube_3_config_dict) + + # Set wrist camera + self.scene.wrist_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand/wrist_cam", + update_period=0.0333, + height=84, + width=84, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(0.025, 0.0, 0.0), rot=(0.707, 0.0, 0.0, 0.707), convention="ros"), + ) + + # Set table view camera + self.scene.table_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/table_cam", + update_period=0.0333, + height=84, + width=84, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) + ), + offset=CameraCfg.OffsetCfg(pos=(1.0, 0.0, 0.33), rot=(-0.3799, 0.5963, 0.5963, -0.3799), convention="ros"), + ) + + # Listens to the required transforms + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.1034], + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + name="tool_rightfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_leftfinger", + name="tool_leftfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + ], + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py new file mode 100644 index 00000000000..2dba904f87a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the lift environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py new file mode 100644 index 00000000000..f6c351f8ae1 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py @@ -0,0 +1,191 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import math +import random +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, AssetBase +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def set_default_joint_pose( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + default_pose: torch.Tensor, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + # Set the default pose for robots in all envs + asset = env.scene[asset_cfg.name] + asset.data.default_joint_pos = torch.tensor(default_pose, device=env.device).repeat(env.num_envs, 1) + + +def randomize_joint_by_gaussian_offset( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + mean: float, + std: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + asset: Articulation = env.scene[asset_cfg.name] + + # Add gaussian noise to joint states + joint_pos = asset.data.default_joint_pos[env_ids].clone() + joint_vel = asset.data.default_joint_vel[env_ids].clone() + joint_pos += math_utils.sample_gaussian(mean, std, joint_pos.shape, joint_pos.device) + + # Clamp joint pos to limits + joint_pos_limits = asset.data.soft_joint_pos_limits[env_ids] + joint_pos = joint_pos.clamp_(joint_pos_limits[..., 0], joint_pos_limits[..., 1]) + + # Don't noise the gripper poses + joint_pos[:, -2:] = asset.data.default_joint_pos[env_ids, -2:] + + # Set into the physics simulation + asset.set_joint_position_target(joint_pos, env_ids=env_ids) + asset.set_joint_velocity_target(joint_vel, env_ids=env_ids) + asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + + +def randomize_scene_lighting_domelight( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + intensity_range: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("light"), +): + asset: AssetBase = env.scene[asset_cfg.name] + light_prim = asset.prims[0] + + # Sample new light intensity + new_intensity = random.uniform(intensity_range[0], intensity_range[1]) + + # Set light intensity to light prim + intensity_attr = light_prim.GetAttribute("inputs:intensity") + intensity_attr.Set(new_intensity) + + +def sample_object_poses( + num_objects: int, + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + pose_list = [] + + for i in range(num_objects): + for j in range(max_sample_tries): + sample = [random.uniform(range[0], range[1]) for range in range_list] + + # Accept pose if it is the first one, or if reached max num tries + if len(pose_list) == 0 or j == max_sample_tries - 1: + pose_list.append(sample) + break + + # Check if pose of object is sufficiently far away from all other objects + separation_check = [math.dist(sample[:3], pose[:3]) > min_separation for pose in pose_list] + if False not in separation_check: + pose_list.append(sample) + break + + return pose_list + + +def randomize_object_pose( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfgs: list[SceneEntityCfg], + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + if env_ids is None: + return + + # Randomize poses in each environment independently + for cur_env in env_ids.tolist(): + pose_list = sample_object_poses( + num_objects=len(asset_cfgs), + min_separation=min_separation, + pose_range=pose_range, + max_sample_tries=max_sample_tries, + ) + + # Randomize pose for each object + for i in range(len(asset_cfgs)): + asset_cfg = asset_cfgs[i] + asset = env.scene[asset_cfg.name] + + # Write pose to simulation + pose_tensor = torch.tensor([pose_list[i]], device=env.device) + positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] + orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) + asset.write_root_pose_to_sim( + torch.cat([positions, orientations], dim=-1), env_ids=torch.tensor([cur_env], device=env.device) + ) + asset.write_root_velocity_to_sim( + torch.zeros(1, 6, device=env.device), env_ids=torch.tensor([cur_env], device=env.device) + ) + + +def randomize_rigid_objects_in_focus( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfgs: list[SceneEntityCfg], + out_focus_state: torch.Tensor, + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + if env_ids is None: + return + + # List of rigid objects in focus for each env (dim = [num_envs, num_rigid_objects]) + env.rigid_objects_in_focus = [] + + for cur_env in env_ids.tolist(): + # Sample in focus object poses + pose_list = sample_object_poses( + num_objects=len(asset_cfgs), + min_separation=min_separation, + pose_range=pose_range, + max_sample_tries=max_sample_tries, + ) + + selected_ids = [] + for asset_idx in range(len(asset_cfgs)): + asset_cfg = asset_cfgs[asset_idx] + asset = env.scene[asset_cfg.name] + + # Randomly select an object to bring into focus + object_id = random.randint(0, asset.num_objects - 1) + selected_ids.append(object_id) + + # Create object state tensor + object_states = torch.stack([out_focus_state] * asset.num_objects).to(device=env.device) + pose_tensor = torch.tensor([pose_list[asset_idx]], device=env.device) + positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] + orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) + object_states[object_id, 0:3] = positions + object_states[object_id, 3:7] = orientations + + asset.write_object_state_to_sim( + object_state=object_states, env_ids=torch.tensor([cur_env], device=env.device) + ) + + env.rigid_objects_in_focus.append(selected_ids) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py new file mode 100644 index 00000000000..23e64f94af2 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/observations.py @@ -0,0 +1,331 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformer + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def cube_positions_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The position of the cubes in the world frame.""" + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + return torch.cat((cube_1.data.root_pos_w, cube_2.data.root_pos_w, cube_3.data.root_pos_w), dim=1) + + +def instance_randomize_cube_positions_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The position of the cubes in the world frame.""" + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + + cube_1_pos_w = [] + cube_2_pos_w = [] + cube_3_pos_w = [] + for env_id in range(env.num_envs): + cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3]) + cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3]) + cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3]) + cube_1_pos_w = torch.stack(cube_1_pos_w) + cube_2_pos_w = torch.stack(cube_2_pos_w) + cube_3_pos_w = torch.stack(cube_3_pos_w) + + return torch.cat((cube_1_pos_w, cube_2_pos_w, cube_3_pos_w), dim=1) + + +def cube_orientations_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +): + """The orientation of the cubes in the world frame.""" + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + return torch.cat((cube_1.data.root_quat_w, cube_2.data.root_quat_w, cube_3.data.root_quat_w), dim=1) + + +def instance_randomize_cube_orientations_in_world_frame( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), +) -> torch.Tensor: + """The orientation of the cubes in the world frame.""" + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + + cube_1_quat_w = [] + cube_2_quat_w = [] + cube_3_quat_w = [] + for env_id in range(env.num_envs): + cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4]) + cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4]) + cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4]) + cube_1_quat_w = torch.stack(cube_1_quat_w) + cube_2_quat_w = torch.stack(cube_2_quat_w) + cube_3_quat_w = torch.stack(cube_3_quat_w) + + return torch.cat((cube_1_quat_w, cube_2_quat_w, cube_3_quat_w), dim=1) + + +def object_obs( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +): + """ + Object observations (in world frame): + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper to cube_1, + gripper to cube_2, + gripper to cube_3, + cube_1 to cube_2, + cube_2 to cube_3, + cube_1 to cube_3, + """ + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + + cube_1_pos_w = cube_1.data.root_pos_w + cube_1_quat_w = cube_1.data.root_quat_w + + cube_2_pos_w = cube_2.data.root_pos_w + cube_2_quat_w = cube_2.data.root_quat_w + + cube_3_pos_w = cube_3.data.root_pos_w + cube_3_quat_w = cube_3.data.root_quat_w + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + gripper_to_cube_1 = cube_1_pos_w - ee_pos_w + gripper_to_cube_2 = cube_2_pos_w - ee_pos_w + gripper_to_cube_3 = cube_3_pos_w - ee_pos_w + + cube_1_to_2 = cube_1_pos_w - cube_2_pos_w + cube_2_to_3 = cube_2_pos_w - cube_3_pos_w + cube_1_to_3 = cube_1_pos_w - cube_3_pos_w + + return torch.cat( + ( + cube_1_pos_w - env.scene.env_origins, + cube_1_quat_w, + cube_2_pos_w - env.scene.env_origins, + cube_2_quat_w, + cube_3_pos_w - env.scene.env_origins, + cube_3_quat_w, + gripper_to_cube_1, + gripper_to_cube_2, + gripper_to_cube_3, + cube_1_to_2, + cube_2_to_3, + cube_1_to_3, + ), + dim=1, + ) + + +def instance_randomize_object_obs( + env: ManagerBasedRLEnv, + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +): + """ + Object observations (in world frame): + cube_1 pos, + cube_1 quat, + cube_2 pos, + cube_2 quat, + cube_3 pos, + cube_3 quat, + gripper to cube_1, + gripper to cube_2, + gripper to cube_3, + cube_1 to cube_2, + cube_2 to cube_3, + cube_1 to cube_3, + """ + if not hasattr(env, "rigid_objects_in_focus"): + return torch.full((env.num_envs, 9), fill_value=-1) + + cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name] + cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name] + cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + + cube_1_pos_w = [] + cube_2_pos_w = [] + cube_3_pos_w = [] + cube_1_quat_w = [] + cube_2_quat_w = [] + cube_3_quat_w = [] + for env_id in range(env.num_envs): + cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3]) + cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3]) + cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3]) + cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4]) + cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4]) + cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4]) + cube_1_pos_w = torch.stack(cube_1_pos_w) + cube_2_pos_w = torch.stack(cube_2_pos_w) + cube_3_pos_w = torch.stack(cube_3_pos_w) + cube_1_quat_w = torch.stack(cube_1_quat_w) + cube_2_quat_w = torch.stack(cube_2_quat_w) + cube_3_quat_w = torch.stack(cube_3_quat_w) + + ee_pos_w = ee_frame.data.target_pos_w[:, 0, :] + gripper_to_cube_1 = cube_1_pos_w - ee_pos_w + gripper_to_cube_2 = cube_2_pos_w - ee_pos_w + gripper_to_cube_3 = cube_3_pos_w - ee_pos_w + + cube_1_to_2 = cube_1_pos_w - cube_2_pos_w + cube_2_to_3 = cube_2_pos_w - cube_3_pos_w + cube_1_to_3 = cube_1_pos_w - cube_3_pos_w + + return torch.cat( + ( + cube_1_pos_w - env.scene.env_origins, + cube_1_quat_w, + cube_2_pos_w - env.scene.env_origins, + cube_2_quat_w, + cube_3_pos_w - env.scene.env_origins, + cube_3_quat_w, + gripper_to_cube_1, + gripper_to_cube_2, + gripper_to_cube_3, + cube_1_to_2, + cube_2_to_3, + cube_1_to_3, + ), + dim=1, + ) + + +def ee_frame_pos(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor: + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_pos = ee_frame.data.target_pos_w[:, 0, :] - env.scene.env_origins[:, 0:3] + + return ee_frame_pos + + +def ee_frame_quat(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor: + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_frame_quat = ee_frame.data.target_quat_w[:, 0, :] + + return ee_frame_quat + + +def gripper_pos(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + robot: Articulation = env.scene[robot_cfg.name] + finger_joint_1 = robot.data.joint_pos[:, -1].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, -2].clone().unsqueeze(1) + + return torch.cat((finger_joint_1, finger_joint_2), dim=1) + + +def object_grasped( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + ee_frame_cfg: SceneEntityCfg, + object_cfg: SceneEntityCfg, + diff_threshold: float = 0.06, + gripper_open_val: torch.tensor = torch.tensor([0.04]), + gripper_threshold: float = 0.005, +) -> torch.Tensor: + """Check if an object is grasped by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + + object_pos = object.data.root_pos_w + end_effector_pos = ee_frame.data.target_pos_w[:, 0, :] + pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) + + grasped = torch.logical_and( + pose_diff < diff_threshold, + torch.abs(robot.data.joint_pos[:, -1] - gripper_open_val.to(env.device)) > gripper_threshold, + ) + grasped = torch.logical_and( + grasped, torch.abs(robot.data.joint_pos[:, -2] - gripper_open_val.to(env.device)) > gripper_threshold + ) + + return grasped + + +def object_stacked( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + upper_object_cfg: SceneEntityCfg, + lower_object_cfg: SceneEntityCfg, + xy_threshold: float = 0.05, + height_threshold: float = 0.005, + height_diff: float = 0.0468, + gripper_open_val: torch.tensor = torch.tensor([0.04]), +) -> torch.Tensor: + """Check if an object is stacked by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + upper_object: RigidObject = env.scene[upper_object_cfg.name] + lower_object: RigidObject = env.scene[lower_object_cfg.name] + + pos_diff = upper_object.data.root_pos_w - lower_object.data.root_pos_w + height_dist = torch.linalg.vector_norm(pos_diff[:, 2:], dim=1) + xy_dist = torch.linalg.vector_norm(pos_diff[:, :2], dim=1) + + stacked = torch.logical_and(xy_dist < xy_threshold, (height_dist - height_diff) < height_threshold) + + stacked = torch.logical_and( + torch.isclose(robot.data.joint_pos[:, -1], gripper_open_val.to(env.device), atol=1e-4, rtol=1e-4), stacked + ) + stacked = torch.logical_and( + torch.isclose(robot.data.joint_pos[:, -2], gripper_open_val.to(env.device), atol=1e-4, rtol=1e-4), stacked + ) + + return stacked diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py new file mode 100644 index 00000000000..09e43f7083a --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/mdp/terminations.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the lift task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def cubes_stacked( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"), + cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"), + cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"), + xy_threshold: float = 0.05, + height_threshold: float = 0.005, + height_diff: float = 0.0468, + gripper_open_val: torch.tensor = torch.tensor([0.04]), + atol=0.0001, + rtol=0.0001, +): + robot: Articulation = env.scene[robot_cfg.name] + cube_1: RigidObject = env.scene[cube_1_cfg.name] + cube_2: RigidObject = env.scene[cube_2_cfg.name] + cube_3: RigidObject = env.scene[cube_3_cfg.name] + + pos_diff_c12 = cube_1.data.root_pos_w - cube_2.data.root_pos_w + pos_diff_c23 = cube_2.data.root_pos_w - cube_3.data.root_pos_w + + # Compute cube position difference in x-y plane + xy_dist_c12 = torch.norm(pos_diff_c12[:, :2], dim=1) + xy_dist_c23 = torch.norm(pos_diff_c23[:, :2], dim=1) + + # Compute cube height difference + h_dist_c12 = torch.norm(pos_diff_c12[:, 2:], dim=1) + h_dist_c23 = torch.norm(pos_diff_c23[:, 2:], dim=1) + + # Check cube positions + stacked = torch.logical_and(xy_dist_c12 < xy_threshold, xy_dist_c23 < xy_threshold) + stacked = torch.logical_and(h_dist_c12 - height_diff < height_threshold, stacked) + stacked = torch.logical_and(h_dist_c23 - height_diff < height_threshold, stacked) + + # Check gripper positions + stacked = torch.logical_and( + torch.isclose(robot.data.joint_pos[:, -1], gripper_open_val.to(env.device), atol=atol, rtol=rtol), stacked + ) + stacked = torch.logical_and( + torch.isclose(robot.data.joint_pos[:, -2], gripper_open_val.to(env.device), atol=atol, rtol=rtol), stacked + ) + + return stacked diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py new file mode 100644 index 00000000000..87ee59179d3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py @@ -0,0 +1,204 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the lift scene with a robot and a object. + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the target object, robot and end-effector frames + """ + + # robots: will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # end-effector sensor: will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + + # Table + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), + spawn=GroundPlaneCfg(), + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object = ObsTerm(func=mdp.object_obs) + cube_positions = ObsTerm(func=mdp.cube_positions_in_world_frame) + cube_orientations = ObsTerm(func=mdp.cube_orientations_in_world_frame) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class RGBCameraPolicyCfg(ObsGroup): + """Observations for policy group with RGB images.""" + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp_1 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_2"), + }, + ) + stack_1 = ObsTerm( + func=mdp.object_stacked, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "upper_object_cfg": SceneEntityCfg("cube_2"), + "lower_object_cfg": SceneEntityCfg("cube_1"), + }, + ) + grasp_2 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_3"), + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + rgb_camera: RGBCameraPolicyCfg = RGBCameraPolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + cube_1_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("cube_1")} + ) + + cube_2_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("cube_2")} + ) + + cube_3_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("cube_3")} + ) + + success = DoneTerm(func=mdp.cubes_stacked) + + +@configclass +class StackEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the stacking environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=4096, env_spacing=2.5, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + commands = None + rewards = None + events = None + curriculum = None + + xr: XrCfg = XrCfg( + anchor_pos=(-0.1, -0.5, -1.05), + anchor_rot=(0.866, 0, 0, -0.5), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 5 + self.episode_length_s = 30.0 + # simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = 2 + + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py new file mode 100644 index 00000000000..c18a372b0a3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/manipulation/stack/stack_instance_randomize_env_cfg.py @@ -0,0 +1,162 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the lift scene with a robot and a object. + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the target object, robot and end-effector frames + """ + + # robots: will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # end-effector sensor: will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + + # Cameras + wrist_cam: CameraCfg = MISSING + table_cam: CameraCfg = MISSING + + # Table + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), + spawn=GroundPlaneCfg(), + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object = ObsTerm(func=mdp.instance_randomize_object_obs) + cube_positions = ObsTerm(func=mdp.instance_randomize_cube_positions_in_world_frame) + cube_orientations = ObsTerm(func=mdp.instance_randomize_cube_orientations_in_world_frame) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class RGBCameraPolicyCfg(ObsGroup): + """Observations for policy group with RGB images.""" + + table_cam = ObsTerm( + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False} + ) + wrist_cam = ObsTerm( + func=mdp.image, params={"sensor_cfg": SceneEntityCfg("wrist_cam"), "data_type": "rgb", "normalize": False} + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + rgb_camera: RGBCameraPolicyCfg = RGBCameraPolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class StackInstanceRandomizeEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the stacking environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=4096, env_spacing=2.5, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + commands = None + rewards = None + events = None + curriculum = None + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 5 + self.episode_length_s = 30.0 + # simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = self.decimation + + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/__init__.py new file mode 100644 index 00000000000..dbd0d1d831f --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Navigation environments.""" + +from .config import anymal_c diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/__init__.py new file mode 100644 index 00000000000..ede9798a230 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for navigation environments.""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py new file mode 100644 index 00000000000..9599e17828c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/__init__.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Navigation-Flat-Anymal-C-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.navigation_env_cfg:NavigationEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:NavigationEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Navigation-Flat-Anymal-C-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.navigation_env_cfg:NavigationEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:NavigationEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", + }, +) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py new file mode 100644 index 00000000000..2227acce701 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..4e2ff6a9f03 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class NavigationEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 8 + max_iterations = 1500 + save_interval = 50 + experiment_name = "anymal_c_navigation" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=0.5, + actor_hidden_dims=[128, 128], + critic_hidden_dims=[128, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml new file mode 100644 index 00000000000..9b98aa7910c --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: -0.6931471805599453 + network: + - name: net + input: STATES + layers: [128, 128] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [128, 128] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 8 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1.0 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "anymal_c_navigation" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 12000 + environment_info: log diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py new file mode 100644 index 00000000000..7eb08e43cdd --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/config/anymal_c/navigation_env_cfg.py @@ -0,0 +1,165 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +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.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +import isaaclab_tasks.manager_based.navigation.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.config.anymal_c.flat_env_cfg import AnymalCFlatEnvCfg + +LOW_LEVEL_ENV_CFG = AnymalCFlatEnvCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (-0.0, 0.0), + "y": (-0.0, 0.0), + "z": (-0.0, 0.0), + "roll": (-0.0, 0.0), + "pitch": (-0.0, 0.0), + "yaw": (-0.0, 0.0), + }, + }, + ) + + +@configclass +class ActionsCfg: + """Action terms for the MDP.""" + + pre_trained_policy_action: mdp.PreTrainedPolicyActionCfg = mdp.PreTrainedPolicyActionCfg( + asset_name="robot", + policy_path=f"{ISAACLAB_NUCLEUS_DIR}/Policies/ANYmal-C/Blind/policy.pt", + low_level_decimation=4, + low_level_actions=LOW_LEVEL_ENV_CFG.actions.joint_pos, + low_level_observations=LOW_LEVEL_ENV_CFG.observations.policy, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel) + projected_gravity = ObsTerm(func=mdp.projected_gravity) + pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "pose_command"}) + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-400.0) + position_tracking = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.5, + params={"std": 2.0, "command_name": "pose_command"}, + ) + position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.5, + params={"std": 0.2, "command_name": "pose_command"}, + ) + orientation_tracking = RewTerm( + func=mdp.heading_command_error_abs, + weight=-0.2, + params={"command_name": "pose_command"}, + ) + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + pose_command = mdp.UniformPose2dCommandCfg( + asset_name="robot", + simple_heading=False, + resampling_time_range=(8.0, 8.0), + debug_vis=True, + ranges=mdp.UniformPose2dCommandCfg.Ranges(pos_x=(-3.0, 3.0), pos_y=(-3.0, 3.0), heading=(-math.pi, math.pi)), + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + base_contact = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="base"), "threshold": 1.0}, + ) + + +@configclass +class NavigationEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the navigation environment.""" + + # environment settings + scene: SceneEntityCfg = LOW_LEVEL_ENV_CFG.scene + actions: ActionsCfg = ActionsCfg() + observations: ObservationsCfg = ObservationsCfg() + events: EventCfg = EventCfg() + # mdp settings + commands: CommandsCfg = CommandsCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + def __post_init__(self): + """Post initialization.""" + + self.sim.dt = LOW_LEVEL_ENV_CFG.sim.dt + self.sim.render_interval = LOW_LEVEL_ENV_CFG.decimation + self.decimation = LOW_LEVEL_ENV_CFG.decimation * 10 + self.episode_length_s = self.commands.pose_command.resampling_time_range[1] + + if self.scene.height_scanner is not None: + self.scene.height_scanner.update_period = ( + self.actions.pre_trained_policy_action.low_level_decimation * self.sim.dt + ) + if self.scene.contact_forces is not None: + self.scene.contact_forces.update_period = self.sim.dt + + +class NavigationEnvCfg_PLAY(NavigationEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/__init__.py new file mode 100644 index 00000000000..a81648980aa --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .pre_trained_policy_action import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py new file mode 100644 index 00000000000..52bf84f8d59 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/pre_trained_policy_action.py @@ -0,0 +1,193 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from dataclasses import MISSING +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation +from isaaclab.managers import ActionTerm, ActionTermCfg, ObservationGroupCfg, ObservationManager +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import BLUE_ARROW_X_MARKER_CFG, GREEN_ARROW_X_MARKER_CFG +from isaaclab.utils import configclass +from isaaclab.utils.assets import check_file_path, read_file + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class PreTrainedPolicyAction(ActionTerm): + r"""Pre-trained policy action term. + + This action term infers a pre-trained policy and applies the corresponding low-level actions to the robot. + The raw actions correspond to the commands for the pre-trained policy. + + """ + + cfg: PreTrainedPolicyActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: PreTrainedPolicyActionCfg, env: ManagerBasedRLEnv) -> None: + # initialize the action term + super().__init__(cfg, env) + + self.robot: Articulation = env.scene[cfg.asset_name] + + # load policy + if not check_file_path(cfg.policy_path): + raise FileNotFoundError(f"Policy file '{cfg.policy_path}' does not exist.") + file_bytes = read_file(cfg.policy_path) + self.policy = torch.jit.load(file_bytes).to(env.device).eval() + + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + + # prepare low level actions + self._low_level_action_term: ActionTerm = cfg.low_level_actions.class_type(cfg.low_level_actions, env) + self.low_level_actions = torch.zeros(self.num_envs, self._low_level_action_term.action_dim, device=self.device) + + def last_action(): + # reset the low level actions if the episode was reset + if hasattr(env, "episode_length_buf"): + self.low_level_actions[env.episode_length_buf == 0, :] = 0 + return self.low_level_actions + + # remap some of the low level observations to internal observations + cfg.low_level_observations.actions.func = lambda dummy_env: last_action() + cfg.low_level_observations.actions.params = dict() + cfg.low_level_observations.velocity_commands.func = lambda dummy_env: self._raw_actions + cfg.low_level_observations.velocity_commands.params = dict() + + # add the low level observations to the observation manager + self._low_level_obs_manager = ObservationManager({"ll_policy": cfg.low_level_observations}, env) + + self._counter = 0 + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return 3 + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self.raw_actions + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + self._raw_actions[:] = actions + + def apply_actions(self): + if self._counter % self.cfg.low_level_decimation == 0: + low_level_obs = self._low_level_obs_manager.compute_group("ll_policy") + self.low_level_actions[:] = self.policy(low_level_obs) + self._low_level_action_term.process_actions(self.low_level_actions) + self._counter = 0 + self._low_level_action_term.apply_actions() + self._counter += 1 + + """ + Debug visualization. + """ + + def _set_debug_vis_impl(self, debug_vis: bool): + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first tome + if not hasattr(self, "base_vel_goal_visualizer"): + # -- goal + marker_cfg = GREEN_ARROW_X_MARKER_CFG.copy() + marker_cfg.prim_path = "/Visuals/Actions/velocity_goal" + marker_cfg.markers["arrow"].scale = (0.5, 0.5, 0.5) + self.base_vel_goal_visualizer = VisualizationMarkers(marker_cfg) + # -- current + marker_cfg = BLUE_ARROW_X_MARKER_CFG.copy() + marker_cfg.prim_path = "/Visuals/Actions/velocity_current" + marker_cfg.markers["arrow"].scale = (0.5, 0.5, 0.5) + self.base_vel_visualizer = VisualizationMarkers(marker_cfg) + # set their visibility to true + self.base_vel_goal_visualizer.set_visibility(True) + self.base_vel_visualizer.set_visibility(True) + else: + if hasattr(self, "base_vel_goal_visualizer"): + self.base_vel_goal_visualizer.set_visibility(False) + self.base_vel_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # check if robot is initialized + # note: this is needed in-case the robot is de-initialized. we can't access the data + if not self.robot.is_initialized: + return + # get marker location + # -- base state + base_pos_w = self.robot.data.root_pos_w.clone() + base_pos_w[:, 2] += 0.5 + # -- resolve the scales and quaternions + vel_des_arrow_scale, vel_des_arrow_quat = self._resolve_xy_velocity_to_arrow(self.raw_actions[:, :2]) + vel_arrow_scale, vel_arrow_quat = self._resolve_xy_velocity_to_arrow(self.robot.data.root_lin_vel_b[:, :2]) + # display markers + self.base_vel_goal_visualizer.visualize(base_pos_w, vel_des_arrow_quat, vel_des_arrow_scale) + self.base_vel_visualizer.visualize(base_pos_w, vel_arrow_quat, vel_arrow_scale) + + """ + Internal helpers. + """ + + def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Converts the XY base velocity command to arrow direction rotation.""" + # obtain default scale of the marker + default_scale = self.base_vel_goal_visualizer.cfg.markers["arrow"].scale + # arrow-scale + arrow_scale = torch.tensor(default_scale, device=self.device).repeat(xy_velocity.shape[0], 1) + arrow_scale[:, 0] *= torch.linalg.norm(xy_velocity, dim=1) * 3.0 + # arrow-direction + heading_angle = torch.atan2(xy_velocity[:, 1], xy_velocity[:, 0]) + zeros = torch.zeros_like(heading_angle) + arrow_quat = math_utils.quat_from_euler_xyz(zeros, zeros, heading_angle) + # convert everything back from base to world frame + base_quat_w = self.robot.data.root_quat_w + arrow_quat = math_utils.quat_mul(base_quat_w, arrow_quat) + + return arrow_scale, arrow_quat + + +@configclass +class PreTrainedPolicyActionCfg(ActionTermCfg): + """Configuration for pre-trained policy action term. + + See :class:`PreTrainedPolicyAction` for more details. + """ + + class_type: type[ActionTerm] = PreTrainedPolicyAction + """ Class of the action term.""" + asset_name: str = MISSING + """Name of the asset in the environment for which the commands are generated.""" + policy_path: str = MISSING + """Path to the low level policy (.pt files).""" + low_level_decimation: int = 4 + """Decimation factor for the low level action term.""" + low_level_actions: ActionTermCfg = MISSING + """Low level action configuration.""" + low_level_observations: ObservationGroupCfg = MISSING + """Low level observation configuration.""" + debug_vis: bool = True + """Whether to visualize debug information. Defaults to False.""" diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/rewards.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/rewards.py new file mode 100644 index 00000000000..8b05d5469e3 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/manager_based/navigation/mdp/rewards.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def position_command_error_tanh(env: ManagerBasedRLEnv, std: float, command_name: str) -> torch.Tensor: + """Reward position tracking with tanh kernel.""" + command = env.command_manager.get_command(command_name) + des_pos_b = command[:, :3] + distance = torch.norm(des_pos_b, dim=1) + return 1 - torch.tanh(distance / std) + + +def heading_command_error_abs(env: ManagerBasedRLEnv, command_name: str) -> torch.Tensor: + """Penalize tracking orientation error.""" + command = env.command_manager.get_command(command_name) + heading_b = command[:, 3] + return heading_b.abs() diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/__init__.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/__init__.py new file mode 100644 index 00000000000..c38d56dfd06 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/__init__.py @@ -0,0 +1,14 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package with utilities, data collectors and environment wrappers.""" + +from .importer import import_packages +from .parse_cfg import get_checkpoint_path, load_cfg_from_registry, parse_env_cfg diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/hydra.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/hydra.py new file mode 100644 index 00000000000..2869feccec0 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/hydra.py @@ -0,0 +1,113 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with utilities for the hydra configuration system.""" + + +import functools +from collections.abc import Callable + +try: + import hydra + from hydra.core.config_store import ConfigStore + from omegaconf import DictConfig, OmegaConf +except ImportError: + raise ImportError("Hydra is not installed. Please install it by running 'pip install hydra-core'.") + +from isaaclab.envs import DirectRLEnvCfg, ManagerBasedRLEnvCfg +from isaaclab.envs.utils.spaces import replace_env_cfg_spaces_with_strings, replace_strings_with_env_cfg_spaces +from isaaclab.utils import replace_slices_with_strings, replace_strings_with_slices + +from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry + + +def register_task_to_hydra( + task_name: str, agent_cfg_entry_point: str +) -> tuple[ManagerBasedRLEnvCfg | DirectRLEnvCfg, dict]: + """Register the task configuration to the Hydra configuration store. + + This function resolves the configuration file for the environment and agent based on the task's name. + It then registers the configurations to the Hydra configuration store. + + Args: + task_name: The name of the task. + agent_cfg_entry_point: The entry point key to resolve the agent's configuration file. + + Returns: + A tuple containing the parsed environment and agent configuration objects. + """ + # load the configurations + env_cfg = load_cfg_from_registry(task_name, "env_cfg_entry_point") + agent_cfg = None + if agent_cfg_entry_point: + agent_cfg = load_cfg_from_registry(task_name, agent_cfg_entry_point) + # replace gymnasium spaces with strings because OmegaConf does not support them. + # this must be done before converting the env configs to dictionary to avoid internal reinterpretations + env_cfg = replace_env_cfg_spaces_with_strings(env_cfg) + # convert the configs to dictionary + env_cfg_dict = env_cfg.to_dict() + if isinstance(agent_cfg, dict) or agent_cfg is None: + agent_cfg_dict = agent_cfg + else: + agent_cfg_dict = agent_cfg.to_dict() + cfg_dict = {"env": env_cfg_dict, "agent": agent_cfg_dict} + # replace slices with strings because OmegaConf does not support slices + cfg_dict = replace_slices_with_strings(cfg_dict) + # store the configuration to Hydra + ConfigStore.instance().store(name=task_name, node=cfg_dict) + return env_cfg, agent_cfg + + +def hydra_task_config(task_name: str, agent_cfg_entry_point: str) -> Callable: + """Decorator to handle the Hydra configuration for a task. + + This decorator registers the task to Hydra and updates the environment and agent configurations from Hydra parsed + command line arguments. + + Args: + task_name: The name of the task. + agent_cfg_entry_point: The entry point key to resolve the agent's configuration file. + + Returns: + The decorated function with the envrionment's and agent's configurations updated from command line arguments. + """ + + def decorator(func): + @functools.wraps(func) + def wrapper(*args, **kwargs): + # register the task to Hydra + env_cfg, agent_cfg = register_task_to_hydra(task_name.split(":")[-1], agent_cfg_entry_point) + + # define the new Hydra main function + @hydra.main(config_path=None, config_name=task_name.split(":")[-1], version_base="1.3") + def hydra_main(hydra_env_cfg: DictConfig, env_cfg=env_cfg, agent_cfg=agent_cfg): + # convert to a native dictionary + hydra_env_cfg = OmegaConf.to_container(hydra_env_cfg, resolve=True) + # replace string with slices because OmegaConf does not support slices + hydra_env_cfg = replace_strings_with_slices(hydra_env_cfg) + # update the configs with the Hydra command line arguments + env_cfg.from_dict(hydra_env_cfg["env"]) + # replace strings that represent gymnasium spaces because OmegaConf does not support them. + # this must be done after converting the env configs from dictionary to avoid internal reinterpretations + env_cfg = replace_strings_with_env_cfg_spaces(env_cfg) + # get agent configs + if isinstance(agent_cfg, dict) or agent_cfg is None: + agent_cfg = hydra_env_cfg["agent"] + else: + agent_cfg.from_dict(hydra_env_cfg["agent"]) + # call the original function + func(env_cfg, agent_cfg, *args, **kwargs) + + # call the new Hydra main function + hydra_main() + + return wrapper + + return decorator diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/importer.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/importer.py new file mode 100644 index 00000000000..3f4a1064456 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/importer.py @@ -0,0 +1,92 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with utility for importing all modules in a package recursively.""" + +from __future__ import annotations + +import importlib +import pkgutil +import sys + + +def import_packages(package_name: str, blacklist_pkgs: list[str] | None = None): + """Import all sub-packages in a package recursively. + + It is easier to use this function to import all sub-packages in a package recursively + than to manually import each sub-package. + + It replaces the need of the following code snippet on the top of each package's ``__init__.py`` file: + + .. code-block:: python + + import .locomotion.velocity + import .manipulation.reach + import .manipulation.lift + + Args: + package_name: The package name. + blacklist_pkgs: The list of blacklisted packages to skip. Defaults to None, + which means no packages are blacklisted. + """ + # Default blacklist + if blacklist_pkgs is None: + blacklist_pkgs = [] + # Import the package itself + package = importlib.import_module(package_name) + # Import all Python files + for _ in _walk_packages(package.__path__, package.__name__ + ".", blacklist_pkgs=blacklist_pkgs): + pass + + +def _walk_packages( + path: str | None = None, + prefix: str = "", + onerror: callable | None = None, + blacklist_pkgs: list[str] | None = None, +): + """Yields ModuleInfo for all modules recursively on path, or, if path is None, all accessible modules. + + Note: + This function is a modified version of the original ``pkgutil.walk_packages`` function. It adds + the `blacklist_pkgs` argument to skip blacklisted packages. Please refer to the original + ``pkgutil.walk_packages`` function for more details. + """ + if blacklist_pkgs is None: + blacklist_pkgs = [] + + def seen(p, m={}): + if p in m: + return True + m[p] = True # noqa: R503 + + for info in pkgutil.iter_modules(path, prefix): + # check blacklisted + if any([black_pkg_name in info.name for black_pkg_name in blacklist_pkgs]): + continue + + # yield the module info + yield info + + if info.ispkg: + try: + __import__(info.name) + except Exception: + if onerror is not None: + onerror(info.name) + else: + raise + else: + path = getattr(sys.modules[info.name], "__path__", None) or [] + + # don't traverse path items we've seen before + path = [p for p in path if not seen(p)] + + yield from _walk_packages(path, info.name + ".", onerror, blacklist_pkgs) diff --git a/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/parse_cfg.py b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/parse_cfg.py new file mode 100644 index 00000000000..aa2fe9d5842 --- /dev/null +++ b/install/isaaclab_tasks/lib/python3.10/site-packages/isaaclab_tasks/utils/parse_cfg.py @@ -0,0 +1,203 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module with utilities for parsing and loading configurations.""" + + +import gymnasium as gym +import importlib +import inspect +import os +import re +import yaml + +from isaaclab.envs import DirectRLEnvCfg, ManagerBasedRLEnvCfg + + +def load_cfg_from_registry(task_name: str, entry_point_key: str) -> dict | object: + """Load default configuration given its entry point from the gym registry. + + This function loads the configuration object from the gym registry for the given task name. + It supports both YAML and Python configuration files. + + It expects the configuration to be registered in the gym registry as: + + .. code-block:: python + + gym.register( + id="My-Awesome-Task-v0", + ... + kwargs={"env_entry_point_cfg": "path.to.config:ConfigClass"}, + ) + + The parsed configuration object for above example can be obtained as: + + .. code-block:: python + + from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry + + cfg = load_cfg_from_registry("My-Awesome-Task-v0", "env_entry_point_cfg") + + Args: + task_name: The name of the environment. + entry_point_key: The entry point key to resolve the configuration file. + + Returns: + The parsed configuration object. If the entry point is a YAML file, it is parsed into a dictionary. + If the entry point is a Python class, it is instantiated and returned. + + Raises: + ValueError: If the entry point key is not available in the gym registry for the task. + """ + # obtain the configuration entry point + cfg_entry_point = gym.spec(task_name.split(":")[-1]).kwargs.get(entry_point_key) + # check if entry point exists + if cfg_entry_point is None: + raise ValueError( + f"Could not find configuration for the environment: '{task_name}'." + f" Please check that the gym registry has the entry point: '{entry_point_key}'." + ) + # parse the default config file + if isinstance(cfg_entry_point, str) and cfg_entry_point.endswith(".yaml"): + if os.path.exists(cfg_entry_point): + # absolute path for the config file + config_file = cfg_entry_point + else: + # resolve path to the module location + mod_name, file_name = cfg_entry_point.split(":") + mod_path = os.path.dirname(importlib.import_module(mod_name).__file__) + # obtain the configuration file path + config_file = os.path.join(mod_path, file_name) + # load the configuration + print(f"[INFO]: Parsing configuration from: {config_file}") + with open(config_file, encoding="utf-8") as f: + cfg = yaml.full_load(f) + else: + if callable(cfg_entry_point): + # resolve path to the module location + mod_path = inspect.getfile(cfg_entry_point) + # load the configuration + cfg_cls = cfg_entry_point() + elif isinstance(cfg_entry_point, str): + # resolve path to the module location + mod_name, attr_name = cfg_entry_point.split(":") + mod = importlib.import_module(mod_name) + cfg_cls = getattr(mod, attr_name) + else: + cfg_cls = cfg_entry_point + # load the configuration + print(f"[INFO]: Parsing configuration from: {cfg_entry_point}") + if callable(cfg_cls): + cfg = cfg_cls() + else: + cfg = cfg_cls + return cfg + + +def parse_env_cfg( + task_name: str, device: str = "cuda:0", num_envs: int | None = None, use_fabric: bool | None = None +) -> ManagerBasedRLEnvCfg | DirectRLEnvCfg: + """Parse configuration for an environment and override based on inputs. + + Args: + task_name: The name of the environment. + device: The device to run the simulation on. Defaults to "cuda:0". + num_envs: Number of environments to create. Defaults to None, in which case it is left unchanged. + use_fabric: Whether to enable/disable fabric interface. If false, all read/write operations go through USD. + This slows down the simulation but allows seeing the changes in the USD through the USD stage. + Defaults to None, in which case it is left unchanged. + + Returns: + The parsed configuration object. + + Raises: + RuntimeError: If the configuration for the task is not a class. We assume users always use a class for the + environment configuration. + """ + # load the default configuration + cfg = load_cfg_from_registry(task_name.split(":")[-1], "env_cfg_entry_point") + + # check that it is not a dict + # we assume users always use a class for the configuration + if isinstance(cfg, dict): + raise RuntimeError(f"Configuration for the task: '{task_name}' is not a class. Please provide a class.") + + # simulation device + cfg.sim.device = device + # disable fabric to read/write through USD + if use_fabric is not None: + cfg.sim.use_fabric = use_fabric + # number of environments + if num_envs is not None: + cfg.scene.num_envs = num_envs + + return cfg + + +def get_checkpoint_path( + log_path: str, run_dir: str = ".*", checkpoint: str = ".*", other_dirs: list[str] = None, sort_alpha: bool = True +) -> str: + """Get path to the model checkpoint in input directory. + + The checkpoint file is resolved as: ``//<*other_dirs>/``, where the + :attr:`other_dirs` are intermediate folder names to concatenate. These cannot be regex expressions. + + If :attr:`run_dir` and :attr:`checkpoint` are regex expressions then the most recent (highest alphabetical order) + run and checkpoint are selected. To disable this behavior, set the flag :attr:`sort_alpha` to False. + + Args: + log_path: The log directory path to find models in. + run_dir: The regex expression for the name of the directory containing the run. Defaults to the most + recent directory created inside :attr:`log_path`. + other_dirs: The intermediate directories between the run directory and the checkpoint file. Defaults to + None, which implies that checkpoint file is directly under the run directory. + checkpoint: The regex expression for the model checkpoint file. Defaults to the most recent + torch-model saved in the :attr:`run_dir` directory. + sort_alpha: Whether to sort the runs by alphabetical order. Defaults to True. + If False, the folders in :attr:`run_dir` are sorted by the last modified time. + + Returns: + The path to the model checkpoint. + + Raises: + ValueError: When no runs are found in the input directory. + ValueError: When no checkpoints are found in the input directory. + + """ + # check if runs present in directory + try: + # find all runs in the directory that math the regex expression + runs = [ + os.path.join(log_path, run) for run in os.scandir(log_path) if run.is_dir() and re.match(run_dir, run.name) + ] + # sort matched runs by alphabetical order (latest run should be last) + if sort_alpha: + runs.sort() + else: + runs = sorted(runs, key=os.path.getmtime) + # create last run file path + if other_dirs is not None: + run_path = os.path.join(runs[-1], *other_dirs) + else: + run_path = runs[-1] + except IndexError: + raise ValueError(f"No runs present in the directory: '{log_path}' match: '{run_dir}'.") + + # list all model checkpoints in the directory + model_checkpoints = [f for f in os.listdir(run_path) if re.match(checkpoint, f)] + # check if any checkpoints are present + if len(model_checkpoints) == 0: + raise ValueError(f"No checkpoints in the directory: '{run_path}' match '{checkpoint}'.") + # sort alphabetically while ensuring that *_10 comes after *_9 + model_checkpoints.sort(key=lambda m: f"{m:0>15}") + # get latest matched checkpoint file + checkpoint_file = model_checkpoints[-1] + + return os.path.join(run_path, checkpoint_file) diff --git a/install/local_setup.bash b/install/local_setup.bash new file mode 100644 index 00000000000..03f00256c1a --- /dev/null +++ b/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.ps1 b/install/local_setup.ps1 new file mode 100644 index 00000000000..6f68c8dede9 --- /dev/null +++ b/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/install/local_setup.sh b/install/local_setup.sh new file mode 100644 index 00000000000..403940ab82c --- /dev/null +++ b/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/lscaglioni/IsaacLab-nomadz/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/mnt/storage/shared/miniconda/bin/python" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.zsh b/install/local_setup.zsh new file mode 100644 index 00000000000..b6487102f24 --- /dev/null +++ b/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/install/setup.bash b/install/setup.bash new file mode 100644 index 00000000000..10ea0f7c07f --- /dev/null +++ b/install/setup.bash @@ -0,0 +1,31 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.ps1 b/install/setup.ps1 new file mode 100644 index 00000000000..558e9b9e627 --- /dev/null +++ b/install/setup.ps1 @@ -0,0 +1,29 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/install/setup.sh b/install/setup.sh new file mode 100644 index 00000000000..c3d42bca086 --- /dev/null +++ b/install/setup.sh @@ -0,0 +1,39 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/lscaglioni/IsaacLab-nomadz/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/install/setup.zsh b/install/setup.zsh new file mode 100644 index 00000000000..54799fde6f8 --- /dev/null +++ b/install/setup.zsh @@ -0,0 +1,31 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/isaaclab.bat b/isaaclab.bat index 0a6cd9f7361..f47364e00b3 100644 --- a/isaaclab.bat +++ b/isaaclab.bat @@ -511,14 +511,8 @@ if "%arg%"=="-i" ( call :extract_python_exe echo [INFO] Using python from: !python_exe! REM Loop through all arguments - mimic shift - set "allArgs=" - for %%a in (%*) do ( - REM Append each argument to the variable, skip the first one - if defined skip ( - set "allArgs=!allArgs! %%a" - ) else ( - set "skip=1" - ) + for /f "tokens=1,* delims= " %%a in ("%*") do ( + set "allArgs=%%b" ) call !python_exe! !allArgs! goto :end @@ -527,14 +521,8 @@ if "%arg%"=="-i" ( call :extract_python_exe echo [INFO] Using python from: !python_exe! REM Loop through all arguments - mimic shift - set "allArgs=" - for %%a in (%*) do ( - REM Append each argument to the variable, skip the first one - if defined skip ( - set "allArgs=!allArgs! %%a" - ) else ( - set "skip=1" - ) + for /f "tokens=1,* delims= " %%a in ("%*") do ( + set "allArgs=%%b" ) call !python_exe! !allArgs! goto :end diff --git a/isaaclab.sh b/isaaclab.sh index 3c8cf33a05d..20508df6bf5 100755 --- a/isaaclab.sh +++ b/isaaclab.sh @@ -96,28 +96,47 @@ is_docker() { [[ "$(hostname)" == *"."* ]] } +# check if running on ARM architecture +is_arm() { + [[ "$(uname -m)" == "aarch64" ]] || [[ "$(uname -m)" == "arm64" ]] +} + ensure_cuda_torch() { - local py="$1" - local -r TORCH_VER="2.7.0" - local -r TV_VER="0.22.0" - local -r CUDA_TAG="cu128" - local -r PYTORCH_INDEX="https://download.pytorch.org/whl/${CUDA_TAG}" - local torch_ver - - if "$py" -m pip show torch >/dev/null 2>&1; then - torch_ver="$("$py" -m pip show torch 2>/dev/null | awk -F': ' '/^Version/{print $2}')" - echo "[INFO] Found PyTorch version ${torch_ver}." - if [[ "$torch_ver" != "${TORCH_VER}+${CUDA_TAG}" ]]; then - echo "[INFO] Replacing PyTorch ${torch_ver} → ${TORCH_VER}+${CUDA_TAG}..." - "$py" -m pip uninstall -y torch torchvision torchaudio >/dev/null 2>&1 || true - "$py" -m pip install "torch==${TORCH_VER}" "torchvision==${TV_VER}" --index-url "${PYTORCH_INDEX}" + local py="$1" + + # base base index for torch + local base_index="https://download.pytorch.org/whl" + + # choose pins per arch + local torch_ver tv_ver cuda_ver + if is_arm; then + torch_ver="2.9.0" + tv_ver="0.24.0" + cuda_ver="130" else - echo "[INFO] PyTorch ${TORCH_VER}+${CUDA_TAG} already installed." + torch_ver="2.7.0" + tv_ver="0.22.0" + cuda_ver="128" + fi + + local index="${base_index}/cu${cuda_ver}" + local want_torch="${torch_ver}+cu${cuda_ver}" + + # check current torch version (may be empty) + local cur="" + if "$py" -m pip show torch >/dev/null 2>&1; then + cur="$("$py" -m pip show torch 2>/dev/null | awk -F': ' '/^Version/{print $2}')" + fi + + # skip install if version is already satisfied + if [[ "$cur" == "$want_torch" ]]; then + return 0 fi - else - echo "[INFO] Installing PyTorch ${TORCH_VER}+${CUDA_TAG}..." - "$py" -m pip install "torch==${TORCH_VER}" "torchvision==${TV_VER}" --index-url "${PYTORCH_INDEX}" - fi + + # clean install torch + echo "[INFO] Installing torch==${torch_ver} and torchvision==${tv_ver} (cu${cuda_ver}) from ${index}..." + "$py" -m pip uninstall -y torch torchvision torchaudio >/dev/null 2>&1 || true + "$py" -m pip install -U --index-url "${index}" "torch==${torch_ver}" "torchvision==${tv_ver}" } # extract isaac sim path @@ -214,6 +233,65 @@ install_isaaclab_extension() { fi } +# Resolve Torch-bundled libgomp and prepend to LD_PRELOAD, once per shell session +write_torch_gomp_hooks() { + mkdir -p "${CONDA_PREFIX}/etc/conda/activate.d" "${CONDA_PREFIX}/etc/conda/deactivate.d" + + # activation: resolve Torch's libgomp via this env's Python and prepend to LD_PRELOAD + cat > "${CONDA_PREFIX}/etc/conda/activate.d/torch_gomp.sh" <<'EOS' +# Resolve Torch-bundled libgomp and prepend to LD_PRELOAD (quiet + idempotent) +: "${_IL_PREV_LD_PRELOAD:=${LD_PRELOAD-}}" + +__gomp="$("$CONDA_PREFIX/bin/python" - <<'PY' 2>/dev/null || true +import pathlib +try: + import torch + p = pathlib.Path(torch.__file__).parent / 'lib' / 'libgomp.so.1' + print(p if p.exists() else "", end="") +except Exception: + pass +PY +)" + +if [ -n "$__gomp" ] && [ -r "$__gomp" ]; then + case ":${LD_PRELOAD:-}:" in + *":$__gomp:"*) : ;; # already present + *) export LD_PRELOAD="$__gomp${LD_PRELOAD:+:$LD_PRELOAD}";; + esac +fi +unset __gomp +EOS + + # deactivation: restore original LD_PRELOAD + cat > "${CONDA_PREFIX}/etc/conda/deactivate.d/torch_gomp_unset.sh" <<'EOS' +# restore LD_PRELOAD to pre-activation value +if [ -v _IL_PREV_LD_PRELOAD ]; then + export LD_PRELOAD="$_IL_PREV_LD_PRELOAD" + unset _IL_PREV_LD_PRELOAD +fi +EOS +} + +# Temporarily unset LD_PRELOAD (ARM only) for a block of commands +begin_arm_install_sandbox() { + if is_arm && [[ -n "${LD_PRELOAD:-}" ]]; then + export _IL_SAVED_LD_PRELOAD="$LD_PRELOAD" + unset LD_PRELOAD + echo "[INFO] ARM install sandbox: temporarily unsetting LD_PRELOAD for installation." + fi + # ensure we restore even if a command fails (set -e) + trap 'end_arm_install_sandbox' EXIT +} + +end_arm_install_sandbox() { + if [[ -n "${_IL_SAVED_LD_PRELOAD:-}" ]]; then + export LD_PRELOAD="$_IL_SAVED_LD_PRELOAD" + unset _IL_SAVED_LD_PRELOAD + fi + # remove trap so later exits don’t re-run restore + trap - EXIT +} + # setup anaconda environment for Isaac Lab setup_conda_env() { # get environment name from input @@ -245,7 +323,7 @@ setup_conda_env() { echo "[INFO] Detected Isaac Sim 4.5 → forcing python=3.10" sed -i 's/^ - python=3\.11/ - python=3.10/' "${ISAACLAB_PATH}/environment.yml" else - echo "[INFO] Isaac Sim 5.0, installing python=3.11" + echo "[INFO] Isaac Sim >= 5.0 detected, installing python=3.11" fi conda env create -y --file ${ISAACLAB_PATH}/environment.yml -n ${env_name} @@ -278,6 +356,7 @@ setup_conda_env() { 'export RESOURCE_NAME="IsaacSim"' \ '' > ${CONDA_PREFIX}/etc/conda/activate.d/setenv.sh + write_torch_gomp_hooks # check if we have _isaac_sim directory -> if so that means binaries were installed. # we need to setup conda variables to load the binaries local isaacsim_setup_conda_env_script=${ISAACLAB_PATH}/_isaac_sim/setup_conda_env.sh @@ -386,8 +465,12 @@ while [[ $# -gt 0 ]]; do # install the python packages in IsaacLab/source directory echo "[INFO] Installing extensions inside the Isaac Lab repository..." python_exe=$(extract_python_exe) - # check if pytorch is installed and its version - # install pytorch with cuda 12.8 for blackwell support + + # if on ARM arch, temporarily clear LD_PRELOAD + # LD_PRELOAD is restored below, after installation + begin_arm_install_sandbox + + # install pytorch (version based on arch) ensure_cuda_torch ${python_exe} # recursively look into directories and install them # this does not check dependencies between extensions @@ -417,6 +500,10 @@ while [[ $# -gt 0 ]]; do # in some rare cases, torch might not be installed properly by setup.py, add one more check here # can prevent that from happening ensure_cuda_torch ${python_exe} + + # restore LD_PRELOAD if we cleared it + end_arm_install_sandbox + # check if we are inside a docker container or are building a docker image # in that case don't setup VSCode since it asks for EULA agreement which triggers user interaction if is_docker; then @@ -470,11 +557,16 @@ while [[ $# -gt 0 ]]; do if [ -n "${CONDA_DEFAULT_ENV}" ]; then export PYTHONPATH=${cache_pythonpath} fi + shift # past argument # exit neatly break ;; -p|--python) + # ensures Kit loads Isaac Sim’s icon instead of a generic icon on aarch64 + if is_arm; then + export RESOURCE_NAME="${RESOURCE_NAME:-IsaacSim}" + fi # run the python provided by isaacsim python_exe=$(extract_python_exe) echo "[INFO] Using python from: ${python_exe}" diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 00000000000..e69de29bb2d diff --git a/log/latest b/log/latest new file mode 120000 index 00000000000..b57d247c77c --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/log/latest_build b/log/latest_build new file mode 120000 index 00000000000..67972087e36 --- /dev/null +++ b/log/latest_build @@ -0,0 +1 @@ +build_2025-06-13_13-34-24 \ No newline at end of file diff --git a/nao.urdf b/nao.urdf new file mode 100644 index 00000000000..03c6d6d5056 --- /dev/null +++ b/nao.urdf @@ -0,0 +1,1393 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_meshes/meshes/V40/HeadPitch.dae b/nao_meshes/meshes/V40/HeadPitch.dae new file mode 100644 index 00000000000..7b5ca9ab1c2 --- /dev/null +++ b/nao_meshes/meshes/V40/HeadPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a3094625b825014872b7994c372611bc70b984fc791b661d460ec184fc325e3 +size 1152698 diff --git a/nao_meshes/meshes/V40/HeadPitch_010.stl b/nao_meshes/meshes/V40/HeadPitch_010.stl new file mode 100644 index 00000000000..f7d4abf236b Binary files /dev/null and b/nao_meshes/meshes/V40/HeadPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/HeadYaw.dae b/nao_meshes/meshes/V40/HeadYaw.dae new file mode 100644 index 00000000000..3294b32f58b --- /dev/null +++ b/nao_meshes/meshes/V40/HeadYaw.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f71e5a070ae151a15e6a1211f1e1c5571c654b80f521df5d7ffd498c9eaafe8c +size 15859 diff --git a/nao_meshes/meshes/V40/HeadYaw_010.stl b/nao_meshes/meshes/V40/HeadYaw_010.stl new file mode 100644 index 00000000000..064ec3268cc Binary files /dev/null and b/nao_meshes/meshes/V40/HeadYaw_010.stl differ diff --git a/nao_meshes/meshes/V40/LAnklePitch.dae b/nao_meshes/meshes/V40/LAnklePitch.dae new file mode 100644 index 00000000000..335e3f1c07b --- /dev/null +++ b/nao_meshes/meshes/V40/LAnklePitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b0edddc22b2e057210cbc7d535bc8e5046642e70f764ea8d086916e6154b757d +size 24930 diff --git a/nao_meshes/meshes/V40/LAnklePitch_010.stl b/nao_meshes/meshes/V40/LAnklePitch_010.stl new file mode 100644 index 00000000000..3e131386b41 Binary files /dev/null and b/nao_meshes/meshes/V40/LAnklePitch_010.stl differ diff --git a/nao_meshes/meshes/V40/LAnkleRoll.dae b/nao_meshes/meshes/V40/LAnkleRoll.dae new file mode 100644 index 00000000000..4f9ff51d2a6 --- /dev/null +++ b/nao_meshes/meshes/V40/LAnkleRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:023b8de16d3a3527d744b75f7b561c5e2289bb2523f9da395ec8a510b6a02870 +size 496187 diff --git a/nao_meshes/meshes/V40/LAnkleRoll_010.stl b/nao_meshes/meshes/V40/LAnkleRoll_010.stl new file mode 100644 index 00000000000..361cd5cf089 Binary files /dev/null and b/nao_meshes/meshes/V40/LAnkleRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/LElbowRoll.dae b/nao_meshes/meshes/V40/LElbowRoll.dae new file mode 100644 index 00000000000..62a7bed231c --- /dev/null +++ b/nao_meshes/meshes/V40/LElbowRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ddc3c7dac20655aa6c14515ab2e345ef5bf56195afc5c2e7d92a52650356100 +size 118290 diff --git a/nao_meshes/meshes/V40/LElbowRoll_010.stl b/nao_meshes/meshes/V40/LElbowRoll_010.stl new file mode 100644 index 00000000000..2854868578e Binary files /dev/null and b/nao_meshes/meshes/V40/LElbowRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/LElbowRoll_010.zip b/nao_meshes/meshes/V40/LElbowRoll_010.zip new file mode 100644 index 00000000000..5623489108d Binary files /dev/null and b/nao_meshes/meshes/V40/LElbowRoll_010.zip differ diff --git a/nao_meshes/meshes/V40/LFinger11.dae b/nao_meshes/meshes/V40/LFinger11.dae new file mode 100644 index 00000000000..998574bf0e1 --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger11.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3c08ff71fb4e234bd793c0ffb6e65adafb709f64e7e268cd48635f47089254a +size 155414 diff --git a/nao_meshes/meshes/V40/LFinger11_010.stl b/nao_meshes/meshes/V40/LFinger11_010.stl new file mode 100644 index 00000000000..088beaa2cee Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger11_010.stl differ diff --git a/nao_meshes/meshes/V40/LFinger12.dae b/nao_meshes/meshes/V40/LFinger12.dae new file mode 100644 index 00000000000..d56b640fe0b --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger12.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b9d97e6d8fc8de86fba593859936c9279061c5ba460abc7edf438d993b7503be +size 152776 diff --git a/nao_meshes/meshes/V40/LFinger12_010.stl b/nao_meshes/meshes/V40/LFinger12_010.stl new file mode 100644 index 00000000000..c88375645a3 Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger12_010.stl differ diff --git a/nao_meshes/meshes/V40/LFinger13.dae b/nao_meshes/meshes/V40/LFinger13.dae new file mode 100644 index 00000000000..371861c39f3 --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger13.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6b9c6ca2357ae3fb5b7b4aa368e2b9c7464141773d1f366a13adf6496d16bae6 +size 135961 diff --git a/nao_meshes/meshes/V40/LFinger13_010.stl b/nao_meshes/meshes/V40/LFinger13_010.stl new file mode 100644 index 00000000000..49aeecafb83 Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger13_010.stl differ diff --git a/nao_meshes/meshes/V40/LFinger21.dae b/nao_meshes/meshes/V40/LFinger21.dae new file mode 100644 index 00000000000..3f5cb7e7a77 --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger21.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:feb44028adf2205f7126124efd9deb5c3717d4da07186d26aba64d7b8a92a6cc +size 155415 diff --git a/nao_meshes/meshes/V40/LFinger21_010.stl b/nao_meshes/meshes/V40/LFinger21_010.stl new file mode 100644 index 00000000000..7283509f57c Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger21_010.stl differ diff --git a/nao_meshes/meshes/V40/LFinger22.dae b/nao_meshes/meshes/V40/LFinger22.dae new file mode 100644 index 00000000000..4e5155f3df9 --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger22.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36dae5cc37a1735d8a1054f93131758607a7ce8cb37fa909fdeeda906a0dceec +size 155542 diff --git a/nao_meshes/meshes/V40/LFinger22_010.stl b/nao_meshes/meshes/V40/LFinger22_010.stl new file mode 100644 index 00000000000..d096469ccb6 Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger22_010.stl differ diff --git a/nao_meshes/meshes/V40/LFinger23.dae b/nao_meshes/meshes/V40/LFinger23.dae new file mode 100644 index 00000000000..2bed3dc4e73 --- /dev/null +++ b/nao_meshes/meshes/V40/LFinger23.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:355cd700a099fe4b164b530f0ab6e5446cee22842df7df79283592eb93b75f5e +size 135683 diff --git a/nao_meshes/meshes/V40/LFinger23_010.stl b/nao_meshes/meshes/V40/LFinger23_010.stl new file mode 100644 index 00000000000..fe488feade8 Binary files /dev/null and b/nao_meshes/meshes/V40/LFinger23_010.stl differ diff --git a/nao_meshes/meshes/V40/LHipPitch.dae b/nao_meshes/meshes/V40/LHipPitch.dae new file mode 100644 index 00000000000..345db18ca5a --- /dev/null +++ b/nao_meshes/meshes/V40/LHipPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:52f2fe7df447b67dadf308f4fa294ec39f482f3919a1f3edfa9a77dac4b92fd5 +size 339962 diff --git a/nao_meshes/meshes/V40/LHipPitch_010.stl b/nao_meshes/meshes/V40/LHipPitch_010.stl new file mode 100644 index 00000000000..030e9e1f747 Binary files /dev/null and b/nao_meshes/meshes/V40/LHipPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/LHipRoll.dae b/nao_meshes/meshes/V40/LHipRoll.dae new file mode 100644 index 00000000000..7662c2755be --- /dev/null +++ b/nao_meshes/meshes/V40/LHipRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:430ca2ad4a9f97a4a681ec91abd17aca91416432a165ce5494ef215d4e5a2508 +size 19101 diff --git a/nao_meshes/meshes/V40/LHipRoll_010.stl b/nao_meshes/meshes/V40/LHipRoll_010.stl new file mode 100644 index 00000000000..ac642066586 Binary files /dev/null and b/nao_meshes/meshes/V40/LHipRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/LHipYawPitch.dae b/nao_meshes/meshes/V40/LHipYawPitch.dae new file mode 100644 index 00000000000..f30667b528d --- /dev/null +++ b/nao_meshes/meshes/V40/LHipYawPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:96860a923bc0c03f283156597f663e1c57bb75c02f0998de2346d42c90c20041 +size 538625 diff --git a/nao_meshes/meshes/V40/LHipYawPitch_010.stl b/nao_meshes/meshes/V40/LHipYawPitch_010.stl new file mode 100644 index 00000000000..d6997c82769 Binary files /dev/null and b/nao_meshes/meshes/V40/LHipYawPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/LKneePitch.dae b/nao_meshes/meshes/V40/LKneePitch.dae new file mode 100644 index 00000000000..daa6dfd2ae3 --- /dev/null +++ b/nao_meshes/meshes/V40/LKneePitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4eaeeac6187460113f5c253c8bda61f4cc7e3835af99408c3e130e9073351bf +size 530638 diff --git a/nao_meshes/meshes/V40/LKneePitch_010.stl b/nao_meshes/meshes/V40/LKneePitch_010.stl new file mode 100644 index 00000000000..66b04267584 Binary files /dev/null and b/nao_meshes/meshes/V40/LKneePitch_010.stl differ diff --git a/nao_meshes/meshes/V40/LShoulderPitch.dae b/nao_meshes/meshes/V40/LShoulderPitch.dae new file mode 100644 index 00000000000..ece49e04815 --- /dev/null +++ b/nao_meshes/meshes/V40/LShoulderPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:448dcb76bc96417c6db475db031724ee5c3a0e2e34ca3c0f741e40aee8f33101 +size 37550 diff --git a/nao_meshes/meshes/V40/LShoulderPitch_010.stl b/nao_meshes/meshes/V40/LShoulderPitch_010.stl new file mode 100644 index 00000000000..8e761ec68af Binary files /dev/null and b/nao_meshes/meshes/V40/LShoulderPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/LShoulderRoll.dae b/nao_meshes/meshes/V40/LShoulderRoll.dae new file mode 100644 index 00000000000..752bdd7b576 --- /dev/null +++ b/nao_meshes/meshes/V40/LShoulderRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e99b886ccb7897cc20f397f99195a4b9bc691c8ba914ce7623f3b26a7c0502c9 +size 597082 diff --git a/nao_meshes/meshes/V40/LShoulderRoll_010.stl b/nao_meshes/meshes/V40/LShoulderRoll_010.stl new file mode 100644 index 00000000000..6fd3957d4ec Binary files /dev/null and b/nao_meshes/meshes/V40/LShoulderRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/LThumb1.dae b/nao_meshes/meshes/V40/LThumb1.dae new file mode 100644 index 00000000000..761a4bf3298 --- /dev/null +++ b/nao_meshes/meshes/V40/LThumb1.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c5017833ef630ccf0fb965a4361caa50e9a01557b0f62c1937f8940605cfc61c +size 155884 diff --git a/nao_meshes/meshes/V40/LThumb1_010.stl b/nao_meshes/meshes/V40/LThumb1_010.stl new file mode 100644 index 00000000000..d784d0ddeb1 Binary files /dev/null and b/nao_meshes/meshes/V40/LThumb1_010.stl differ diff --git a/nao_meshes/meshes/V40/LThumb2.dae b/nao_meshes/meshes/V40/LThumb2.dae new file mode 100644 index 00000000000..628b19bd6f0 --- /dev/null +++ b/nao_meshes/meshes/V40/LThumb2.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ebabc32ec48d4e190d2b376d3d7b40ca763db2d06ad47d02c68630e2bc69da6b +size 138321 diff --git a/nao_meshes/meshes/V40/LThumb2_010.stl b/nao_meshes/meshes/V40/LThumb2_010.stl new file mode 100644 index 00000000000..142741fb683 Binary files /dev/null and b/nao_meshes/meshes/V40/LThumb2_010.stl differ diff --git a/nao_meshes/meshes/V40/LWristYaw.dae b/nao_meshes/meshes/V40/LWristYaw.dae new file mode 100644 index 00000000000..51c124ff3ee --- /dev/null +++ b/nao_meshes/meshes/V40/LWristYaw.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:300b5a7be4cfa6a4901f7c02b37e9ffd3476b48695b36168a3ff68bbc80bc7a1 +size 571278 diff --git a/nao_meshes/meshes/V40/LWristYaw_010.stl b/nao_meshes/meshes/V40/LWristYaw_010.stl new file mode 100644 index 00000000000..12fe18bf9c7 Binary files /dev/null and b/nao_meshes/meshes/V40/LWristYaw_010.stl differ diff --git a/nao_meshes/meshes/V40/RAnklePitch.dae b/nao_meshes/meshes/V40/RAnklePitch.dae new file mode 100644 index 00000000000..03b6cb25664 --- /dev/null +++ b/nao_meshes/meshes/V40/RAnklePitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7ee64fa5c47af91abd5847642df2855fdebebd1ccd242bf4cf867d0ef32f2f2d +size 25160 diff --git a/nao_meshes/meshes/V40/RAnklePitch_010.stl b/nao_meshes/meshes/V40/RAnklePitch_010.stl new file mode 100644 index 00000000000..23afaf696df Binary files /dev/null and b/nao_meshes/meshes/V40/RAnklePitch_010.stl differ diff --git a/nao_meshes/meshes/V40/RAnkleRoll.dae b/nao_meshes/meshes/V40/RAnkleRoll.dae new file mode 100644 index 00000000000..9c52a7cfd23 --- /dev/null +++ b/nao_meshes/meshes/V40/RAnkleRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:16da1bc088b5d64349c2f41d52d15d23c03c42e44b8d543fc4812fe533f31dfc +size 492748 diff --git a/nao_meshes/meshes/V40/RAnkleRoll_010.stl b/nao_meshes/meshes/V40/RAnkleRoll_010.stl new file mode 100644 index 00000000000..c44b29ac985 Binary files /dev/null and b/nao_meshes/meshes/V40/RAnkleRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/RElbowRoll.dae b/nao_meshes/meshes/V40/RElbowRoll.dae new file mode 100644 index 00000000000..482b4f99610 --- /dev/null +++ b/nao_meshes/meshes/V40/RElbowRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9f93e4c2fa59340caabba4250a2933381f025a290d38ab6367eb34112958c16a +size 120144 diff --git a/nao_meshes/meshes/V40/RElbowRoll_010.stl b/nao_meshes/meshes/V40/RElbowRoll_010.stl new file mode 100644 index 00000000000..848b6afec59 Binary files /dev/null and b/nao_meshes/meshes/V40/RElbowRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger11.dae b/nao_meshes/meshes/V40/RFinger11.dae new file mode 100644 index 00000000000..33127e2e49d --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger11.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d66e17a98ada82fe6480c5214ae82d4256ac64f27d97282af79d2b0b8e5a65ba +size 154413 diff --git a/nao_meshes/meshes/V40/RFinger11_010.stl b/nao_meshes/meshes/V40/RFinger11_010.stl new file mode 100644 index 00000000000..95811bbf650 Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger11_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger12.dae b/nao_meshes/meshes/V40/RFinger12.dae new file mode 100644 index 00000000000..e1026e26e76 --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger12.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:267ae2ee0e7de1bba6110e34676cc29b9e5e471a7f8603a3b257444c4dd36a93 +size 156005 diff --git a/nao_meshes/meshes/V40/RFinger12_010.stl b/nao_meshes/meshes/V40/RFinger12_010.stl new file mode 100644 index 00000000000..be7cee53a63 Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger12_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger13.dae b/nao_meshes/meshes/V40/RFinger13.dae new file mode 100644 index 00000000000..3369262c384 --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger13.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:35841a8b727f47fc3079c5c2ca7b5776db001d7b0b28ad64f2a4ce7806666a8d +size 135894 diff --git a/nao_meshes/meshes/V40/RFinger13_010.stl b/nao_meshes/meshes/V40/RFinger13_010.stl new file mode 100644 index 00000000000..a1f2ffbd3bd Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger13_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger21.dae b/nao_meshes/meshes/V40/RFinger21.dae new file mode 100644 index 00000000000..28c540a6955 --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger21.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec311a8f280fd79655f75ffee935f75d629ed85bc7223ac9c7011f460b1f21ca +size 154763 diff --git a/nao_meshes/meshes/V40/RFinger21_010.stl b/nao_meshes/meshes/V40/RFinger21_010.stl new file mode 100644 index 00000000000..4a2af746890 Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger21_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger22.dae b/nao_meshes/meshes/V40/RFinger22.dae new file mode 100644 index 00000000000..a4867949786 --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger22.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11561de08a6dac772cca4160fa3b5087eac8ec36dea7725ba54fa7bd2804ea5d +size 155668 diff --git a/nao_meshes/meshes/V40/RFinger22_010.stl b/nao_meshes/meshes/V40/RFinger22_010.stl new file mode 100644 index 00000000000..4260f54f2cd Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger22_010.stl differ diff --git a/nao_meshes/meshes/V40/RFinger23.dae b/nao_meshes/meshes/V40/RFinger23.dae new file mode 100644 index 00000000000..e688f71cf30 --- /dev/null +++ b/nao_meshes/meshes/V40/RFinger23.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e6cb0b1b6ad198b8d4dfdf59d783c6ff4898d7c9437c133b5858b17bd01ae730 +size 135825 diff --git a/nao_meshes/meshes/V40/RFinger23_010.stl b/nao_meshes/meshes/V40/RFinger23_010.stl new file mode 100644 index 00000000000..663cb1f5f8b Binary files /dev/null and b/nao_meshes/meshes/V40/RFinger23_010.stl differ diff --git a/nao_meshes/meshes/V40/RHipPitch.dae b/nao_meshes/meshes/V40/RHipPitch.dae new file mode 100644 index 00000000000..838e6a18961 --- /dev/null +++ b/nao_meshes/meshes/V40/RHipPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ab887ab7b8e997fd5cd9e1ebedde5f1b12d64313f3c9a8b04643ea3f80b952f0 +size 337346 diff --git a/nao_meshes/meshes/V40/RHipPitch_010.stl b/nao_meshes/meshes/V40/RHipPitch_010.stl new file mode 100644 index 00000000000..9d6ef12e64c Binary files /dev/null and b/nao_meshes/meshes/V40/RHipPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/RHipRoll.dae b/nao_meshes/meshes/V40/RHipRoll.dae new file mode 100644 index 00000000000..5be675be7e7 --- /dev/null +++ b/nao_meshes/meshes/V40/RHipRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70d9d366654653b772f7e13c91be02bb74dedfadb1bfdf1f78f4ea94c7fd43a3 +size 19071 diff --git a/nao_meshes/meshes/V40/RHipRoll_010.stl b/nao_meshes/meshes/V40/RHipRoll_010.stl new file mode 100644 index 00000000000..36d67b9b4e7 Binary files /dev/null and b/nao_meshes/meshes/V40/RHipRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/RHipYawPitch.dae b/nao_meshes/meshes/V40/RHipYawPitch.dae new file mode 100644 index 00000000000..0b6eee06dc4 --- /dev/null +++ b/nao_meshes/meshes/V40/RHipYawPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b4ad5b6e0f3bacc91ef7f42c4589ea60feb59a373e7c5903cb630cdb85daa071 +size 536986 diff --git a/nao_meshes/meshes/V40/RHipYawPitch_010.stl b/nao_meshes/meshes/V40/RHipYawPitch_010.stl new file mode 100644 index 00000000000..6697ebb7b48 Binary files /dev/null and b/nao_meshes/meshes/V40/RHipYawPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/RKneePitch.dae b/nao_meshes/meshes/V40/RKneePitch.dae new file mode 100644 index 00000000000..60f154a97a5 --- /dev/null +++ b/nao_meshes/meshes/V40/RKneePitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5e96805e6dff010c3476098a7a7eceb3deff636228cc22b27264e2f7af247eb4 +size 533541 diff --git a/nao_meshes/meshes/V40/RKneePitch_010.stl b/nao_meshes/meshes/V40/RKneePitch_010.stl new file mode 100644 index 00000000000..54fa06e50b9 Binary files /dev/null and b/nao_meshes/meshes/V40/RKneePitch_010.stl differ diff --git a/nao_meshes/meshes/V40/RShoulderPitch.dae b/nao_meshes/meshes/V40/RShoulderPitch.dae new file mode 100644 index 00000000000..f593c176de6 --- /dev/null +++ b/nao_meshes/meshes/V40/RShoulderPitch.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1a5cf3f758ba131a5210c4feb0fef2157c58cfebd14965a5671d8ea628adbdaf +size 38938 diff --git a/nao_meshes/meshes/V40/RShoulderPitch_010.stl b/nao_meshes/meshes/V40/RShoulderPitch_010.stl new file mode 100644 index 00000000000..8333a48176a Binary files /dev/null and b/nao_meshes/meshes/V40/RShoulderPitch_010.stl differ diff --git a/nao_meshes/meshes/V40/RShoulderRoll.dae b/nao_meshes/meshes/V40/RShoulderRoll.dae new file mode 100644 index 00000000000..486a8d18383 --- /dev/null +++ b/nao_meshes/meshes/V40/RShoulderRoll.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1d8d5f82efafee887bea1626b56cdc1bc43ceeb76a90865e8dbfd4fc7c8b0eba +size 593693 diff --git a/nao_meshes/meshes/V40/RShoulderRoll_010.stl b/nao_meshes/meshes/V40/RShoulderRoll_010.stl new file mode 100644 index 00000000000..3166375fedb Binary files /dev/null and b/nao_meshes/meshes/V40/RShoulderRoll_010.stl differ diff --git a/nao_meshes/meshes/V40/RThumb1.dae b/nao_meshes/meshes/V40/RThumb1.dae new file mode 100644 index 00000000000..c68d0d0b816 --- /dev/null +++ b/nao_meshes/meshes/V40/RThumb1.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2d89596f7687080af2b490d5c9ec6956a9ac5bcf3eddef98212748df028866cd +size 153525 diff --git a/nao_meshes/meshes/V40/RThumb1_010.stl b/nao_meshes/meshes/V40/RThumb1_010.stl new file mode 100644 index 00000000000..c7a3189cf95 Binary files /dev/null and b/nao_meshes/meshes/V40/RThumb1_010.stl differ diff --git a/nao_meshes/meshes/V40/RThumb2.dae b/nao_meshes/meshes/V40/RThumb2.dae new file mode 100644 index 00000000000..e7e6be64180 --- /dev/null +++ b/nao_meshes/meshes/V40/RThumb2.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8838680b7916725cdd159c3b095588a9b3abc104671a56de302fa663d97d48a2 +size 136926 diff --git a/nao_meshes/meshes/V40/RThumb2_010.stl b/nao_meshes/meshes/V40/RThumb2_010.stl new file mode 100644 index 00000000000..85777b605d8 Binary files /dev/null and b/nao_meshes/meshes/V40/RThumb2_010.stl differ diff --git a/nao_meshes/meshes/V40/RWristYaw.dae b/nao_meshes/meshes/V40/RWristYaw.dae new file mode 100644 index 00000000000..51c124ff3ee --- /dev/null +++ b/nao_meshes/meshes/V40/RWristYaw.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:300b5a7be4cfa6a4901f7c02b37e9ffd3476b48695b36168a3ff68bbc80bc7a1 +size 571278 diff --git a/nao_meshes/meshes/V40/RWristYaw_010.stl b/nao_meshes/meshes/V40/RWristYaw_010.stl new file mode 100644 index 00000000000..abc0c67b3e4 Binary files /dev/null and b/nao_meshes/meshes/V40/RWristYaw_010.stl differ diff --git a/nao_meshes/meshes/V40/Torso.dae b/nao_meshes/meshes/V40/Torso.dae new file mode 100644 index 00000000000..3e4e3b991a0 --- /dev/null +++ b/nao_meshes/meshes/V40/Torso.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2f13cc75900f7fa4c6fe14b76a89f051f21e08e738b8dad3d9d4732383e06f3f +size 1060701 diff --git a/nao_meshes/meshes/V40/Torso_010.stl b/nao_meshes/meshes/V40/Torso_010.stl new file mode 100644 index 00000000000..d4873f52893 Binary files /dev/null and b/nao_meshes/meshes/V40/Torso_010.stl differ diff --git a/scripts/benchmarks/benchmark_non_rl.py b/scripts/benchmarks/benchmark_non_rl.py index 295436e75f3..adc797e7f5e 100644 --- a/scripts/benchmarks/benchmark_non_rl.py +++ b/scripts/benchmarks/benchmark_non_rl.py @@ -113,6 +113,14 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # override configurations with non-hydra CLI arguments env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + env_cfg.seed = args_cli.seed + + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) # process distributed world_size = 1 diff --git a/scripts/benchmarks/benchmark_rlgames.py b/scripts/benchmarks/benchmark_rlgames.py index 2394228efc9..c142af86185 100644 --- a/scripts/benchmarks/benchmark_rlgames.py +++ b/scripts/benchmarks/benchmark_rlgames.py @@ -75,7 +75,7 @@ from isaaclab.envs import DirectMARLEnvCfg, DirectRLEnvCfg, ManagerBasedRLEnvCfg from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml from isaaclab_rl.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper @@ -128,6 +128,17 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # override configurations with non-hydra CLI arguments env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) + + # update agent device to match simulation device + if args_cli.device is not None: + agent_cfg["params"]["config"]["device"] = args_cli.device + agent_cfg["params"]["config"]["device_name"] = args_cli.device # randomly sample a seed if seed = -1 if args_cli.seed == -1: @@ -168,8 +179,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_root_path, log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_root_path, log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_root_path, log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_root_path, log_dir, "params", "agent.pkl"), agent_cfg) # read configurations about the agent-training rl_device = agent_cfg["params"]["config"]["device"] diff --git a/scripts/benchmarks/benchmark_rsl_rl.py b/scripts/benchmarks/benchmark_rsl_rl.py index 56207f4fe82..506559fb442 100644 --- a/scripts/benchmarks/benchmark_rsl_rl.py +++ b/scripts/benchmarks/benchmark_rsl_rl.py @@ -75,7 +75,7 @@ from isaaclab.envs import DirectMARLEnvCfg, DirectRLEnvCfg, ManagerBasedRLEnvCfg from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper @@ -140,6 +140,12 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # note: certain randomizations occur in the environment initialization so we set the seed here env_cfg.seed = agent_cfg.seed env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) # multi-gpu training configuration world_rank = 0 @@ -207,8 +213,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) benchmark.set_phase("sim_runtime") diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index 6de2b9dfec5..71eac60e07b 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -4,7 +4,6 @@ # SPDX-License-Identifier: BSD-3-Clause import argparse -import numpy as np from isaaclab.app import AppLauncher @@ -22,6 +21,7 @@ """Rest everything follows.""" +import numpy as np import torch import isaaclab.sim as sim_utils diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 021ee5ff80f..32f125b194f 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -19,7 +19,11 @@ "--teleop_device", type=str, default="keyboard", - help="Device for interacting with environment. Examples: keyboard, spacemouse, gamepad, handtracking, manusvive", + help=( + "Teleop device. Set here (legacy) or via the environment config. If using the environment config, pass the" + " device key/name defined under 'teleop_devices' (it can be a custom name, not necessarily 'handtracking')." + " Built-ins: keyboard, spacemouse, gamepad. Not all tasks support all built-ins." + ), ) parser.add_argument("--task", type=str, default=None, help="Name of the task.") parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") @@ -66,6 +70,7 @@ from isaaclab_tasks.utils import parse_env_cfg if args_cli.enable_pinocchio: + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 diff --git a/scripts/imitation_learning/locomanipulation_sdg/generate_data.py b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py new file mode 100644 index 00000000000..b80e0992ce4 --- /dev/null +++ b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py @@ -0,0 +1,774 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to replay demonstrations with Isaac Lab environments.""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +import os + +from isaaclab.app import AppLauncher + +# Launch Isaac Lab +parser = argparse.ArgumentParser(description="Locomanipulation SDG") +parser.add_argument("--task", type=str, help="The Isaac Lab locomanipulation SDG task to load for data generation.") +parser.add_argument("--dataset", type=str, help="The static manipulation dataset recorded via teleoperation.") +parser.add_argument("--output_file", type=str, help="The file name for the generated output dataset.") +parser.add_argument( + "--lift_step", + type=int, + help=( + "The step index in the input recording where the robot is ready to lift the object. Aka, where the grasp is" + " finished." + ), +) +parser.add_argument( + "--navigate_step", + type=int, + help=( + "The step index in the input recording where the robot is ready to navigate. Aka, where it has finished" + " lifting the object" + ), +) +parser.add_argument("--demo", type=str, default=None, help="The demo in the input dataset to use.") +parser.add_argument("--num_runs", type=int, default=1, help="The number of trajectories to generate.") +parser.add_argument( + "--draw_visualization", type=bool, default=False, help="Draw the occupancy map and path planning visualization." +) +parser.add_argument( + "--angular_gain", + type=float, + default=2.0, + help=( + "The angular gain to use for determining an angular control velocity when driving the robot during navigation." + ), +) +parser.add_argument( + "--linear_gain", + type=float, + default=1.0, + help="The linear gain to use for determining the linear control velocity when driving the robot during navigation.", +) +parser.add_argument( + "--linear_max", type=float, default=1.0, help="The maximum linear control velocity allowable during navigation." +) +parser.add_argument( + "--distance_threshold", + type=float, + default=0.2, + help="The distance threshold in meters to perform state transitions between navigation and manipulation tasks.", +) +parser.add_argument( + "--following_offset", + type=float, + default=0.6, + help=( + "The target point offset distance used for local path following during navigation. A larger value will result" + " in smoother trajectories, but may cut path corners." + ), +) +parser.add_argument( + "--angle_threshold", + type=float, + default=0.2, + help=( + "The angle threshold in radians to determine when the robot can move forward or transition between navigation" + " and manipulation tasks." + ), +) +parser.add_argument( + "--approach_distance", + type=float, + default=0.5, + help="An offset distance added to the destination to allow a buffer zone for reliably approaching the goal.", +) +parser.add_argument( + "--randomize_placement", + type=bool, + default=True, + help="Whether or not to randomize the placement of fixtures in the scene upon environment initialization.", +) +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +if args_cli.enable_pinocchio: + # Import pinocchio before AppLauncher to force the use of the version installed by IsaacLab and not the one installed by Isaac Sim + # pinocchio is required by the Pink IK controllers and the GR1T2 retargeter + import pinocchio # noqa: F401 + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import enum +import gymnasium as gym +import random +import torch + +import omni.kit + +from isaaclab.utils import configclass +from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler + +import isaaclab_mimic.locomanipulation_sdg.envs # noqa: F401 +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGOutputData +from isaaclab_mimic.locomanipulation_sdg.envs.locomanipulation_sdg_env import LocomanipulationSDGEnv +from isaaclab_mimic.locomanipulation_sdg.occupancy_map_utils import ( + OccupancyMap, + merge_occupancy_maps, + occupancy_map_add_to_stage, +) +from isaaclab_mimic.locomanipulation_sdg.path_utils import ParameterizedPath, plan_path +from isaaclab_mimic.locomanipulation_sdg.scene_utils import RelativePose, place_randomly +from isaaclab_mimic.locomanipulation_sdg.transform_utils import transform_inv, transform_mul, transform_relative_pose + +from isaaclab_tasks.utils import parse_env_cfg + + +class LocomanipulationSDGDataGenerationState(enum.IntEnum): + """States for the locomanipulation SDG data generation state machine.""" + + GRASP_OBJECT = 0 + """Robot grasps object at start position""" + + LIFT_OBJECT = 1 + """Robot lifts object while stationary""" + + NAVIGATE = 2 + """Robot navigates to approach position with object""" + + APPROACH = 3 + """Robot approaches final goal position""" + + DROP_OFF_OBJECT = 4 + """Robot places object at end position""" + + DONE = 5 + """Task completed""" + + +@configclass +class LocomanipulationSDGControlConfig: + """Configuration for navigation control parameters.""" + + angular_gain: float = 2.0 + """Proportional gain for angular velocity control""" + + linear_gain: float = 1.0 + """Proportional gain for linear velocity control""" + + linear_max: float = 1.0 + """Maximum allowed linear velocity (m/s)""" + + distance_threshold: float = 0.1 + """Distance threshold for state transitions (m)""" + + following_offset: float = 0.6 + """Look-ahead distance for path following (m)""" + + angle_threshold: float = 0.2 + """Angular threshold for orientation control (rad)""" + + approach_distance: float = 1.0 + """Buffer distance from final goal (m)""" + + +def compute_navigation_velocity( + current_pose: torch.Tensor, target_xy: torch.Tensor, config: LocomanipulationSDGControlConfig +) -> tuple[torch.Tensor, torch.Tensor]: + """Compute linear and angular velocities for navigation control. + + Args: + current_pose: Current robot pose [x, y, yaw] + target_xy: Target position [x, y] + config: Navigation control configuration + + Returns: + Tuple of (linear_velocity, angular_velocity) + """ + current_xy = current_pose[:2] + current_yaw = current_pose[2] + + # Compute position and orientation errors + delta_xy = target_xy - current_xy + delta_distance = torch.sqrt(torch.sum(delta_xy**2)) + + target_yaw = torch.arctan2(delta_xy[1], delta_xy[0]) + delta_yaw = target_yaw - current_yaw + # Normalize angle to [-π, π] + delta_yaw = (delta_yaw + torch.pi) % (2 * torch.pi) - torch.pi + + # Compute control commands + angular_velocity = config.angular_gain * delta_yaw + linear_velocity = torch.clip(config.linear_gain * delta_distance, 0.0, config.linear_max) / ( + 1 + torch.abs(angular_velocity) + ) + + return linear_velocity, angular_velocity + + +def load_and_transform_recording_data( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + reference_pose: torch.Tensor, + target_pose: torch.Tensor, +) -> tuple[torch.Tensor, torch.Tensor]: + """Load recording data and transform hand targets to current reference frame. + + Args: + env: The locomanipulation SDG environment + input_episode_data: Input episode data from static manipulation + recording_step: Current step in the recording + reference_pose: Original reference pose for the hand targets + target_pose: Current target pose to transform to + + Returns: + Tuple of transformed (left_hand_pose, right_hand_pose) + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + if recording_item is None: + return None, None + + left_hand_pose = transform_relative_pose(recording_item.left_hand_pose_target, reference_pose, target_pose)[0] + right_hand_pose = transform_relative_pose(recording_item.right_hand_pose_target, reference_pose, target_pose)[0] + + return left_hand_pose, right_hand_pose + + +def setup_navigation_scene( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + approach_distance: float, + randomize_placement: bool = True, +) -> tuple[OccupancyMap, ParameterizedPath, RelativePose, RelativePose]: + """Set up the navigation scene with occupancy map and path planning. + + Args: + env: The locomanipulation SDG environment + input_episode_data: Input episode data + approach_distance: Buffer distance from final goal + randomize_placement: Whether to randomize fixture placement + + Returns: + Tuple of (occupancy_map, path_helper, base_goal, base_goal_approach) + """ + # Create base occupancy map + occupancy_map = merge_occupancy_maps([ + OccupancyMap.make_empty(start=(-7, -7), end=(7, 7), resolution=0.05), + env.get_start_fixture().get_occupancy_map(), + ]) + + # Randomize fixture placement if enabled + if randomize_placement: + fixtures = [env.get_end_fixture()] + env.get_obstacle_fixtures() + for fixture in fixtures: + place_randomly(fixture, occupancy_map.buffered_meters(1.0)) + occupancy_map = merge_occupancy_maps([occupancy_map, fixture.get_occupancy_map()]) + + # Compute goal poses from initial state + initial_state = env.load_input_data(input_episode_data, 0) + base_goal = RelativePose( + relative_pose=transform_mul(transform_inv(initial_state.fixture_pose), initial_state.base_pose), + parent=env.get_end_fixture(), + ) + base_goal_approach = RelativePose( + relative_pose=torch.tensor([-approach_distance, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]), parent=base_goal + ) + + # Plan navigation path + base_path = plan_path( + start=env.get_base(), end=base_goal_approach, occupancy_map=occupancy_map.buffered_meters(0.15) + ) + base_path_helper = ParameterizedPath(base_path) + + return occupancy_map, base_path_helper, base_goal, base_goal_approach + + +def handle_grasp_state( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + lift_step: int, + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState]: + """Handle the GRASP_OBJECT state logic. + + Args: + env: The environment + input_episode_data: Input episode data + recording_step: Current recording step + lift_step: Step to transition to lift phase + output_data: Output data to populate + + Returns: + Tuple of (next_recording_step, next_state) + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + + # Set control targets - robot stays stationary during grasping + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.GRASP_OBJECT) + output_data.recording_step = recording_step + output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0]) + + # Transform hand poses relative to object + output_data.left_hand_pose_target = transform_relative_pose( + recording_item.left_hand_pose_target, recording_item.object_pose, env.get_object().get_pose() + )[0] + output_data.right_hand_pose_target = transform_relative_pose( + recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target + output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target + + # Update state + + next_recording_step = recording_step + 1 + next_state = ( + LocomanipulationSDGDataGenerationState.LIFT_OBJECT + if next_recording_step > lift_step + else LocomanipulationSDGDataGenerationState.GRASP_OBJECT + ) + + return next_recording_step, next_state + + +def handle_lift_state( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + navigate_step: int, + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState]: + """Handle the LIFT_OBJECT state logic. + + Args: + env: The environment + input_episode_data: Input episode data + recording_step: Current recording step + navigate_step: Step to transition to navigation phase + output_data: Output data to populate + + Returns: + Tuple of (next_recording_step, next_state) + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + + # Set control targets - robot stays stationary during lifting + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.LIFT_OBJECT) + output_data.recording_step = recording_step + output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0]) + + # Transform hand poses relative to base + output_data.left_hand_pose_target = transform_relative_pose( + recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.right_hand_pose_target = transform_relative_pose( + recording_item.right_hand_pose_target, recording_item.object_pose, env.get_object().get_pose() + )[0] + output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target + output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target + + # Update state + next_recording_step = recording_step + 1 + next_state = ( + LocomanipulationSDGDataGenerationState.NAVIGATE + if next_recording_step > navigate_step + else LocomanipulationSDGDataGenerationState.LIFT_OBJECT + ) + + return next_recording_step, next_state + + +def handle_navigate_state( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + base_path_helper: ParameterizedPath, + base_goal_approach: RelativePose, + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> LocomanipulationSDGDataGenerationState: + """Handle the NAVIGATE state logic. + + Args: + env: The environment + input_episode_data: Input episode data + recording_step: Current recording step + base_path_helper: Parameterized path for navigation + base_goal_approach: Approach pose goal + config: Navigation control configuration + output_data: Output data to populate + + Returns: + Next state + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + current_pose = env.get_base().get_pose_2d()[0] + + # Find target point along path using pure pursuit algorithm + _, nearest_path_length, _, _ = base_path_helper.find_nearest(current_pose[:2]) + target_xy = base_path_helper.get_point_by_distance(distance=nearest_path_length + config.following_offset) + + # Compute navigation velocities + linear_velocity, angular_velocity = compute_navigation_velocity(current_pose, target_xy, config) + + # Set control targets + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.NAVIGATE) + output_data.recording_step = recording_step + output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) + + # Transform hand poses relative to base + output_data.left_hand_pose_target = transform_relative_pose( + recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.right_hand_pose_target = transform_relative_pose( + recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target + output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target + + # Check if close enough to approach goal to transition + goal_xy = base_goal_approach.get_pose_2d()[0, :2] + distance_to_goal = torch.sqrt(torch.sum((current_pose[:2] - goal_xy) ** 2)) + + return ( + LocomanipulationSDGDataGenerationState.APPROACH + if distance_to_goal < config.distance_threshold + else LocomanipulationSDGDataGenerationState.NAVIGATE + ) + + +def handle_approach_state( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + base_goal: RelativePose, + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> LocomanipulationSDGDataGenerationState: + """Handle the APPROACH state logic. + + Args: + env: The environment + input_episode_data: Input episode data + recording_step: Current recording step + base_goal: Final goal pose + config: Navigation control configuration + output_data: Output data to populate + + Returns: + Next state + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + current_pose = env.get_base().get_pose_2d()[0] + + # Navigate directly to final goal position + goal_xy = base_goal.get_pose_2d()[0, :2] + linear_velocity, angular_velocity = compute_navigation_velocity(current_pose, goal_xy, config) + + # Set control targets + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.APPROACH) + output_data.recording_step = recording_step + output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) + + # Transform hand poses relative to base + output_data.left_hand_pose_target = transform_relative_pose( + recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.right_hand_pose_target = transform_relative_pose( + recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose() + )[0] + output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target + output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target + + # Check if close enough to final goal to start drop-off + distance_to_goal = torch.sqrt(torch.sum((current_pose[:2] - goal_xy) ** 2)) + + return ( + LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT + if distance_to_goal < config.distance_threshold + else LocomanipulationSDGDataGenerationState.APPROACH + ) + + +def handle_drop_off_state( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + recording_step: int, + base_goal: RelativePose, + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState | None]: + """Handle the DROP_OFF_OBJECT state logic. + + Args: + env: The environment + input_episode_data: Input episode data + recording_step: Current recording step + base_goal: Final goal pose + config: Navigation control configuration + output_data: Output data to populate + + Returns: + Tuple of (next_recording_step, next_state) + """ + recording_item = env.load_input_data(input_episode_data, recording_step) + if recording_item is None: + return recording_step, None + + # Compute orientation control to face target orientation + current_pose = env.get_base().get_pose_2d()[0] + target_pose = base_goal.get_pose_2d()[0] + current_yaw = current_pose[2] + target_yaw = target_pose[2] + delta_yaw = target_yaw - current_yaw + delta_yaw = (delta_yaw + torch.pi) % (2 * torch.pi) - torch.pi + + angular_velocity = config.angular_gain * delta_yaw + linear_velocity = 0.0 # Stay in place while orienting + + # Set control targets + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT) + output_data.recording_step = recording_step + output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) + + # Transform hand poses relative to end fixture + output_data.left_hand_pose_target = transform_relative_pose( + recording_item.left_hand_pose_target, + recording_item.fixture_pose, + env.get_end_fixture().get_pose(), + )[0] + output_data.right_hand_pose_target = transform_relative_pose( + recording_item.right_hand_pose_target, + recording_item.fixture_pose, + env.get_end_fixture().get_pose(), + )[0] + output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target + output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target + + # Continue playback if orientation is within threshold + next_recording_step = recording_step + 1 if abs(delta_yaw) < config.angle_threshold else recording_step + + return next_recording_step, LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT + + +def populate_output_data( + env: LocomanipulationSDGEnv, + output_data: LocomanipulationSDGOutputData, + base_goal: RelativePose, + base_goal_approach: RelativePose, + base_path: torch.Tensor, +) -> None: + """Populate remaining output data fields. + + Args: + env: The environment + output_data: Output data to populate + base_goal: Final goal pose + base_goal_approach: Approach goal pose + base_path: Planned navigation path + """ + output_data.base_pose = env.get_base().get_pose() + output_data.object_pose = env.get_object().get_pose() + output_data.start_fixture_pose = env.get_start_fixture().get_pose() + output_data.end_fixture_pose = env.get_end_fixture().get_pose() + output_data.base_goal_pose = base_goal.get_pose() + output_data.base_goal_approach_pose = base_goal_approach.get_pose() + output_data.base_path = base_path + + # Collect obstacle poses + obstacle_poses = [] + for obstacle in env.get_obstacle_fixtures(): + obstacle_poses.append(obstacle.get_pose()) + if obstacle_poses: + output_data.obstacle_fixture_poses = torch.cat(obstacle_poses, dim=0)[None, :] + else: + output_data.obstacle_fixture_poses = torch.empty((1, 0, 7)) # Empty tensor with correct shape + + +def replay( + env: LocomanipulationSDGEnv, + input_episode_data: EpisodeData, + lift_step: int, + navigate_step: int, + draw_visualization: bool = False, + angular_gain: float = 2.0, + linear_gain: float = 1.0, + linear_max: float = 1.0, + distance_threshold: float = 0.1, + following_offset: float = 0.6, + angle_threshold: float = 0.2, + approach_distance: float = 1.0, + randomize_placement: bool = True, +) -> None: + """Replay a locomanipulation SDG episode with state machine control. + + This function implements a state machine for locomanipulation SDG, where the robot: + 1. Grasps an object at the start position + 2. Lifts the object while stationary + 3. Navigates with the object to an approach position + 4. Approaches the final goal position + 5. Places the object at the end position + + Args: + env: The locomanipulation SDG environment + input_episode_data: Static manipulation episode data to replay + lift_step: Recording step where lifting phase begins + navigate_step: Recording step where navigation phase begins + draw_visualization: Whether to visualize occupancy map and path + angular_gain: Proportional gain for angular velocity control + linear_gain: Proportional gain for linear velocity control + linear_max: Maximum linear velocity (m/s) + distance_threshold: Distance threshold for state transitions (m) + following_offset: Look-ahead distance for path following (m) + angle_threshold: Angular threshold for orientation control (rad) + approach_distance: Buffer distance from final goal (m) + randomize_placement: Whether to randomize obstacle placement + """ + + # Initialize environment to starting state + env.reset_to(state=input_episode_data.get_initial_state(), env_ids=torch.tensor([0]), is_relative=True) + + # Create navigation control configuration + config = LocomanipulationSDGControlConfig( + angular_gain=angular_gain, + linear_gain=linear_gain, + linear_max=linear_max, + distance_threshold=distance_threshold, + following_offset=following_offset, + angle_threshold=angle_threshold, + approach_distance=approach_distance, + ) + + # Set up navigation scene and path planning + occupancy_map, base_path_helper, base_goal, base_goal_approach = setup_navigation_scene( + env, input_episode_data, approach_distance, randomize_placement + ) + + # Visualize occupancy map and path if requested + if draw_visualization: + occupancy_map_add_to_stage( + occupancy_map, + stage=omni.usd.get_context().get_stage(), + path="/OccupancyMap", + z_offset=0.01, + draw_path=base_path_helper.points, + ) + + # Initialize state machine + output_data = LocomanipulationSDGOutputData() + current_state = LocomanipulationSDGDataGenerationState.GRASP_OBJECT + recording_step = 0 + + # Main simulation loop with state machine + while simulation_app.is_running() and not simulation_app.is_exiting(): + + print(f"Current state: {current_state.name}, Recording step: {recording_step}") + + # Execute state-specific logic using helper functions + if current_state == LocomanipulationSDGDataGenerationState.GRASP_OBJECT: + recording_step, current_state = handle_grasp_state( + env, input_episode_data, recording_step, lift_step, output_data + ) + + elif current_state == LocomanipulationSDGDataGenerationState.LIFT_OBJECT: + recording_step, current_state = handle_lift_state( + env, input_episode_data, recording_step, navigate_step, output_data + ) + + elif current_state == LocomanipulationSDGDataGenerationState.NAVIGATE: + current_state = handle_navigate_state( + env, input_episode_data, recording_step, base_path_helper, base_goal_approach, config, output_data + ) + + elif current_state == LocomanipulationSDGDataGenerationState.APPROACH: + current_state = handle_approach_state( + env, input_episode_data, recording_step, base_goal, config, output_data + ) + + elif current_state == LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT: + recording_step, next_state = handle_drop_off_state( + env, input_episode_data, recording_step, base_goal, config, output_data + ) + if next_state is None: # End of episode data + break + current_state = next_state + + # Populate additional output data fields + populate_output_data(env, output_data, base_goal, base_goal_approach, base_path_helper.points) + + # Attach output data to environment for recording + env._locomanipulation_sdg_output_data = output_data + + # Build and execute action + action = env.build_action_vector( + base_velocity_target=output_data.base_velocity_target, + left_hand_joint_positions_target=output_data.left_hand_joint_positions_target, + right_hand_joint_positions_target=output_data.right_hand_joint_positions_target, + left_hand_pose_target=output_data.left_hand_pose_target, + right_hand_pose_target=output_data.right_hand_pose_target, + ) + + env.step(action) + + +if __name__ == "__main__": + + with torch.no_grad(): + + # Create environment + if args_cli.task is not None: + env_name = args_cli.task.split(":")[-1] + if env_name is None: + raise ValueError("Task/env name was not specified nor found in the dataset.") + + env_cfg = parse_env_cfg(env_name, device=args_cli.device, num_envs=1) + env_cfg.sim.device = "cpu" + env_cfg.recorders.dataset_export_dir_path = os.path.dirname(args_cli.output_file) + env_cfg.recorders.dataset_filename = os.path.basename(args_cli.output_file) + + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + + # Load input data + input_dataset_file_handler = HDF5DatasetFileHandler() + input_dataset_file_handler.open(args_cli.dataset) + + for i in range(args_cli.num_runs): + + if args_cli.demo is None: + demo = random.choice(list(input_dataset_file_handler.get_episode_names())) + else: + demo = args_cli.demo + + input_episode_data = input_dataset_file_handler.load_episode(demo, args_cli.device) + + replay( + env=env, + input_episode_data=input_episode_data, + lift_step=args_cli.lift_step, + navigate_step=args_cli.navigate_step, + draw_visualization=args_cli.draw_visualization, + angular_gain=args_cli.angular_gain, + linear_gain=args_cli.linear_gain, + linear_max=args_cli.linear_max, + distance_threshold=args_cli.distance_threshold, + following_offset=args_cli.following_offset, + angle_threshold=args_cli.angle_threshold, + approach_distance=args_cli.approach_distance, + randomize_placement=args_cli.randomize_placement, + ) + + env.reset() # FIXME: hack to handle missing final recording + env.close() + + simulation_app.close() diff --git a/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py b/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py new file mode 100644 index 00000000000..6981ff803d1 --- /dev/null +++ b/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py @@ -0,0 +1,109 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to visualize navigation datasets. + +Loads a navigation dataset and generates plots showing paths, poses and obstacles. + +Args: + dataset: Path to the HDF5 dataset file containing recorded demonstrations. + output_dir: Directory path where visualization plots will be saved. + figure_size: Size of the generated figures (width, height). + demo_filter: If provided, only visualize specific demo(s). Can be a single demo name or comma-separated list. +""" + +import argparse +import h5py +import matplotlib.pyplot as plt +import os + + +def main(): + """Main function to process dataset and generate visualizations.""" + # add argparse arguments + parser = argparse.ArgumentParser( + description="Visualize navigation dataset from locomanipulation sdg demonstrations." + ) + parser.add_argument( + "--input_file", type=str, help="Path to the HDF5 dataset file containing recorded demonstrations." + ) + parser.add_argument("--output_dir", type=str, help="Directory path where visualization plots will be saved.") + parser.add_argument( + "--figure_size", + type=int, + nargs=2, + default=[20, 20], + help="Size of the generated figures (width, height). Default: [20, 20]", + ) + parser.add_argument( + "--demo_filter", + type=str, + default=None, + help="If provided, only visualize specific demo(s). Can be a single demo name or comma-separated list.", + ) + + # parse the arguments + args = parser.parse_args() + + # Validate inputs + if not os.path.exists(args.input_file): + raise FileNotFoundError(f"Dataset file not found: {args.input_file}") + + # Create output directory if it doesn't exist + os.makedirs(args.output_dir, exist_ok=True) + + # Load dataset + dataset = h5py.File(args.input_file, "r") + + demos = list(dataset["data"].keys()) + + # Filter demos if specified + if args.demo_filter: + filter_demos = [d.strip() for d in args.demo_filter.split(",")] + demos = [d for d in demos if d in filter_demos] + if not demos: + print(f"Warning: No demos found matching filter '{args.demo_filter}'") + return + + print(f"Visualizing {len(demos)} demonstrations...") + + for i, demo in enumerate(demos): + print(f"Processing demo {i + 1}/{len(demos)}: {demo}") + + replay_data = dataset["data"][demo]["locomanipulation_sdg_output_data"] + path = replay_data["base_path"] + base_pose = replay_data["base_pose"] + object_pose = replay_data["object_pose"] + start_pose = replay_data["start_fixture_pose"] + end_pose = replay_data["end_fixture_pose"] + obstacle_poses = replay_data["obstacle_fixture_poses"] + + plt.figure(figsize=args.figure_size) + plt.plot(path[0, :, 0], path[0, :, 1], "r-", label="Target Path", linewidth=2) + plt.plot(base_pose[:, 0], base_pose[:, 1], "g--", label="Base Pose", linewidth=2) + plt.plot(object_pose[:, 0], object_pose[:, 1], "b--", label="Object Pose", linewidth=2) + plt.plot(obstacle_poses[0, :, 0], obstacle_poses[0, :, 1], "ro", label="Obstacles", markersize=8) + + # Add start and end markers + plt.plot(start_pose[0, 0], start_pose[0, 1], "gs", label="Start", markersize=12) + plt.plot(end_pose[0, 0], end_pose[0, 1], "rs", label="End", markersize=12) + + plt.legend(loc="upper right", ncol=1, fontsize=12) + plt.axis("equal") + plt.grid(True, alpha=0.3) + plt.title(f"Navigation Visualization - {demo}", fontsize=16) + plt.xlabel("X Position (m)", fontsize=14) + plt.ylabel("Y Position (m)", fontsize=14) + + output_path = os.path.join(args.output_dir, f"{demo}.png") + plt.savefig(output_path, dpi=150, bbox_inches="tight") + plt.close() # Close the figure to free memory + + dataset.close() + print(f"Visualization complete! Plots saved to: {args.output_dir}") + + +if __name__ == "__main__": + main() diff --git a/scripts/imitation_learning/robomimic/play.py b/scripts/imitation_learning/robomimic/play.py index 4b1476f6bea..4cc327941d0 100644 --- a/scripts/imitation_learning/robomimic/play.py +++ b/scripts/imitation_learning/robomimic/play.py @@ -70,6 +70,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg diff --git a/scripts/imitation_learning/robomimic/train.py b/scripts/imitation_learning/robomimic/train.py index 945c1f40f98..718a18bcbca 100644 --- a/scripts/imitation_learning/robomimic/train.py +++ b/scripts/imitation_learning/robomimic/train.py @@ -84,6 +84,7 @@ # Isaac Lab imports (needed so that environment is registered) import isaaclab_tasks # noqa: F401 +import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 diff --git a/scripts/reinforcement_learning/rl_games/play.py b/scripts/reinforcement_learning/rl_games/play.py index dd2185b82b0..d6faec37316 100644 --- a/scripts/reinforcement_learning/rl_games/play.py +++ b/scripts/reinforcement_learning/rl_games/play.py @@ -95,6 +95,10 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # override configurations with non-hydra CLI arguments env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # update agent device to match simulation device + if args_cli.device is not None: + agent_cfg["params"]["config"]["device"] = args_cli.device + agent_cfg["params"]["config"]["device_name"] = args_cli.device # randomly sample a seed if seed = -1 if args_cli.seed == -1: @@ -130,6 +134,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen resume_path = retrieve_file_path(args_cli.checkpoint) log_dir = os.path.dirname(os.path.dirname(resume_path)) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # wrap around environment for rl-games rl_device = agent_cfg["params"]["config"]["device"] clip_obs = agent_cfg["params"]["env"].get("clip_observations", math.inf) diff --git a/scripts/reinforcement_learning/rl_games/train.py b/scripts/reinforcement_learning/rl_games/train.py index cc1e54b1756..9fd7285558a 100644 --- a/scripts/reinforcement_learning/rl_games/train.py +++ b/scripts/reinforcement_learning/rl_games/train.py @@ -79,9 +79,9 @@ ) from isaaclab.utils.assets import retrieve_file_path from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml -from isaaclab_rl.rl_games import RlGamesGpuEnv, RlGamesVecEnvWrapper +from isaaclab_rl.rl_games import MultiObserver, PbtAlgoObserver, RlGamesGpuEnv, RlGamesVecEnvWrapper import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.hydra import hydra_task_config @@ -95,6 +95,17 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # override configurations with non-hydra CLI arguments env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) + + # update agent device to match simulation device + if args_cli.device is not None: + agent_cfg["params"]["config"]["device"] = args_cli.device + agent_cfg["params"]["config"]["device_name"] = args_cli.device # randomly sample a seed if seed = -1 if args_cli.seed == -1: @@ -127,7 +138,12 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # specify directory for logging experiments config_name = agent_cfg["params"]["config"]["name"] log_root_path = os.path.join("logs", "rl_games", config_name) - log_root_path = os.path.abspath(log_root_path) + if "pbt" in agent_cfg: + if agent_cfg["pbt"]["directory"] == ".": + log_root_path = os.path.abspath(log_root_path) + else: + log_root_path = os.path.join(agent_cfg["pbt"]["directory"], log_root_path) + print(f"[INFO] Logging experiment in directory: {log_root_path}") # specify directory for logging runs log_dir = agent_cfg["params"]["config"].get("full_experiment_name", datetime.now().strftime("%Y-%m-%d_%H-%M-%S")) @@ -141,8 +157,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_root_path, log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_root_path, log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_root_path, log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_root_path, log_dir, "params", "agent.pkl"), agent_cfg) # read configurations about the agent-training rl_device = agent_cfg["params"]["config"]["device"] @@ -160,6 +174,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen "IO descriptors are only supported for manager based RL environments. No IO descriptors will be exported." ) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = os.path.join(log_root_path, log_dir) + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) @@ -192,7 +209,13 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # set number of actors into agent config agent_cfg["params"]["config"]["num_actors"] = env.unwrapped.num_envs # create runner from rl-games - runner = Runner(IsaacAlgoObserver()) + + if "pbt" in agent_cfg and agent_cfg["pbt"]["enabled"]: + observers = MultiObserver([IsaacAlgoObserver(), PbtAlgoObserver(agent_cfg, args_cli)]) + runner = Runner(observers) + else: + runner = Runner(IsaacAlgoObserver()) + runner.load(agent_cfg) # reset the agent and env @@ -213,8 +236,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen monitor_gym=True, save_code=True, ) - wandb.config.update({"env_cfg": env_cfg.to_dict()}) - wandb.config.update({"agent_cfg": agent_cfg}) + if not wandb.run.resumed: + wandb.config.update({"env_cfg": env_cfg.to_dict()}) + wandb.config.update({"agent_cfg": agent_cfg}) if args_cli.checkpoint is not None: runner.run({"train": True, "play": False, "sigma": train_sigma, "checkpoint": resume_path}) diff --git a/scripts/reinforcement_learning/rsl_rl/play.py b/scripts/reinforcement_learning/rsl_rl/play.py index 9e89c6ff318..11ef7399462 100644 --- a/scripts/reinforcement_learning/rsl_rl/play.py +++ b/scripts/reinforcement_learning/rsl_rl/play.py @@ -112,6 +112,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen log_dir = os.path.dirname(resume_path) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) diff --git a/scripts/reinforcement_learning/rsl_rl/train.py b/scripts/reinforcement_learning/rsl_rl/train.py index 33bfc9f63d4..e8e6d706696 100644 --- a/scripts/reinforcement_learning/rsl_rl/train.py +++ b/scripts/reinforcement_learning/rsl_rl/train.py @@ -88,7 +88,7 @@ multi_agent_to_single_agent, ) from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper @@ -118,6 +118,12 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # note: certain randomizations occur in the environment initialization so we set the seed here env_cfg.seed = agent_cfg.seed env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) # multi-gpu training configuration if args_cli.distributed: @@ -150,6 +156,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen "IO descriptors are only supported for manager based RL environments. No IO descriptors will be exported." ) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) @@ -194,8 +203,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) # run training runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True) diff --git a/scripts/reinforcement_learning/sb3/play.py b/scripts/reinforcement_learning/sb3/play.py index 05c52390749..c803c1807ba 100644 --- a/scripts/reinforcement_learning/sb3/play.py +++ b/scripts/reinforcement_learning/sb3/play.py @@ -127,6 +127,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen checkpoint_path = args_cli.checkpoint log_dir = os.path.dirname(checkpoint_path) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) diff --git a/scripts/reinforcement_learning/sb3/train.py b/scripts/reinforcement_learning/sb3/train.py index ba45398f108..a85385c4d38 100644 --- a/scripts/reinforcement_learning/sb3/train.py +++ b/scripts/reinforcement_learning/sb3/train.py @@ -91,7 +91,7 @@ def cleanup_pbar(*args): multi_agent_to_single_agent, ) from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml from isaaclab_rl.sb3 import Sb3VecEnvWrapper, process_sb3_cfg @@ -130,8 +130,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) # save command used to run the script command = " ".join(sys.orig_argv) @@ -152,6 +150,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen "IO descriptors are only supported for manager based RL environments. No IO descriptors will be exported." ) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) diff --git a/scripts/reinforcement_learning/skrl/play.py b/scripts/reinforcement_learning/skrl/play.py index 990ef5b558d..6be6b0eae3b 100644 --- a/scripts/reinforcement_learning/skrl/play.py +++ b/scripts/reinforcement_learning/skrl/play.py @@ -121,6 +121,7 @@ agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" else: agent_cfg_entry_point = args_cli.agent + algorithm = agent_cfg_entry_point.split("_cfg")[0].split("skrl_")[-1].lower() @hydra_task_config(args_cli.task, agent_cfg_entry_point) @@ -165,6 +166,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, expe ) log_dir = os.path.dirname(os.path.dirname(resume_path)) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) diff --git a/scripts/reinforcement_learning/skrl/train.py b/scripts/reinforcement_learning/skrl/train.py index e3399f204b5..4b4947423e1 100644 --- a/scripts/reinforcement_learning/skrl/train.py +++ b/scripts/reinforcement_learning/skrl/train.py @@ -104,7 +104,7 @@ ) from isaaclab.utils.assets import retrieve_file_path from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab.utils.io import dump_yaml from isaaclab_rl.skrl import SkrlVecEnvWrapper @@ -119,6 +119,7 @@ agent_cfg_entry_point = "skrl_cfg_entry_point" if algorithm in ["ppo"] else f"skrl_{algorithm}_cfg_entry_point" else: agent_cfg_entry_point = args_cli.agent + algorithm = agent_cfg_entry_point.split("_cfg")[0].split("skrl_")[-1].lower() @hydra_task_config(args_cli.task, agent_cfg_entry_point) @@ -128,6 +129,13 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + # check for invalid combination of CPU device with distributed training + if args_cli.distributed and args_cli.device is not None and "cpu" in args_cli.device: + raise ValueError( + "Distributed training is not supported when using CPU device. " + "Please use GPU device (e.g., --device cuda) for distributed training." + ) + # multi-gpu training config if args_cli.distributed: env_cfg.sim.device = f"cuda:{app_launcher.local_rank}" @@ -167,8 +175,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # dump the configuration into log-directory dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) - dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) - dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) # get checkpoint path (to resume training) resume_path = retrieve_file_path(args_cli.checkpoint) if args_cli.checkpoint else None @@ -182,6 +188,9 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen "IO descriptors are only supported for manager based RL environments. No IO descriptors will be exported." ) + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) diff --git a/scripts/sim2sim_transfer/config/newton_to_physx_anymal_d.yaml b/scripts/sim2sim_transfer/config/newton_to_physx_anymal_d.yaml new file mode 100644 index 00000000000..bbf4b73dccb --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_anymal_d.yaml @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Joint names in the source physics engine where policy is trained (Newton) +source_joint_names: + - "LF_HAA" + - "LF_HFE" + - "LF_KFE" + - "LH_HAA" + - "LH_HFE" + - "LH_KFE" + - "RF_HAA" + - "RF_HFE" + - "RF_KFE" + - "RH_HAA" + - "RH_HFE" + - "RH_KFE" + +# Joint names in the target physics engine where policy is deployed (PhysX) +target_joint_names: + - "LF_HAA" + - "LH_HAA" + - "RF_HAA" + - "RH_HAA" + - "LF_HFE" + - "LH_HFE" + - "RF_HFE" + - "RH_HFE" + - "LF_KFE" + - "LH_KFE" + - "RF_KFE" + - "RH_KFE" diff --git a/scripts/sim2sim_transfer/config/newton_to_physx_g1.yaml b/scripts/sim2sim_transfer/config/newton_to_physx_g1.yaml new file mode 100644 index 00000000000..3a2343405f3 --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_g1.yaml @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Joint names in the source physics engine where policy is trained (Newton) +source_joint_names: + - "left_hip_pitch_joint" + - "left_hip_roll_joint" + - "left_hip_yaw_joint" + - "left_knee_joint" + - "left_ankle_pitch_joint" + - "left_ankle_roll_joint" + - "right_hip_pitch_joint" + - "right_hip_roll_joint" + - "right_hip_yaw_joint" + - "right_knee_joint" + - "right_ankle_pitch_joint" + - "right_ankle_roll_joint" + - "torso_joint" + - "left_shoulder_pitch_joint" + - "left_shoulder_roll_joint" + - "left_shoulder_yaw_joint" + - "left_elbow_pitch_joint" + - "left_elbow_roll_joint" + - "left_five_joint" + - "left_six_joint" + - "left_three_joint" + - "left_four_joint" + - "left_zero_joint" + - "left_one_joint" + - "left_two_joint" + - "right_shoulder_pitch_joint" + - "right_shoulder_roll_joint" + - "right_shoulder_yaw_joint" + - "right_elbow_pitch_joint" + - "right_elbow_roll_joint" + - "right_five_joint" + - "right_six_joint" + - "right_three_joint" + - "right_four_joint" + - "right_zero_joint" + - "right_one_joint" + - "right_two_joint" + +# Joint names in the target physics engine where policy is deployed (PhysX) +target_joint_names: + - "left_hip_pitch_joint" + - "right_hip_pitch_joint" + - "torso_joint" + - "left_hip_roll_joint" + - "right_hip_roll_joint" + - "left_shoulder_pitch_joint" + - "right_shoulder_pitch_joint" + - "left_hip_yaw_joint" + - "right_hip_yaw_joint" + - "left_shoulder_roll_joint" + - "right_shoulder_roll_joint" + - "left_knee_joint" + - "right_knee_joint" + - "left_shoulder_yaw_joint" + - "right_shoulder_yaw_joint" + - "left_ankle_pitch_joint" + - "right_ankle_pitch_joint" + - "left_elbow_pitch_joint" + - "right_elbow_pitch_joint" + - "left_ankle_roll_joint" + - "right_ankle_roll_joint" + - "left_elbow_roll_joint" + - "right_elbow_roll_joint" + - "left_five_joint" + - "left_three_joint" + - "left_zero_joint" + - "right_five_joint" + - "right_three_joint" + - "right_zero_joint" + - "left_six_joint" + - "left_four_joint" + - "left_one_joint" + - "right_six_joint" + - "right_four_joint" + - "right_one_joint" + - "left_two_joint" + - "right_two_joint" diff --git a/scripts/sim2sim_transfer/config/newton_to_physx_go2.yaml b/scripts/sim2sim_transfer/config/newton_to_physx_go2.yaml new file mode 100644 index 00000000000..143ca36d799 --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_go2.yaml @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Joint names in the source physics engine where policy is trained (Newton) +source_joint_names: + - "FL_hip_joint" + - "FL_thigh_joint" + - "FL_calf_joint" + - "FR_hip_joint" + - "FR_thigh_joint" + - "FR_calf_joint" + - "RL_hip_joint" + - "RL_thigh_joint" + - "RL_calf_joint" + - "RR_hip_joint" + - "RR_thigh_joint" + - "RR_calf_joint" +# Joint names in the target physics engine where policy is deployed (PhysX) +target_joint_names: + - "FL_hip_joint" + - "FR_hip_joint" + - "RL_hip_joint" + - "RR_hip_joint" + - "FL_thigh_joint" + - "FR_thigh_joint" + - "RL_thigh_joint" + - "RR_thigh_joint" + - "FL_calf_joint" + - "FR_calf_joint" + - "RL_calf_joint" + - "RR_calf_joint" diff --git a/scripts/sim2sim_transfer/config/newton_to_physx_h1.yaml b/scripts/sim2sim_transfer/config/newton_to_physx_h1.yaml new file mode 100644 index 00000000000..b88ee333cff --- /dev/null +++ b/scripts/sim2sim_transfer/config/newton_to_physx_h1.yaml @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Joint names in the source physics engine where policy is trained (Newton) +source_joint_names: + - "left_hip_yaw" + - "left_hip_roll" + - "left_hip_pitch" + - "left_knee" + - "left_ankle" + - "right_hip_yaw" + - "right_hip_roll" + - "right_hip_pitch" + - "right_knee" + - "right_ankle" + - "torso" + - "left_shoulder_pitch" + - "left_shoulder_roll" + - "left_shoulder_yaw" + - "left_elbow" + - "right_shoulder_pitch" + - "right_shoulder_roll" + - "right_shoulder_yaw" + - "right_elbow" + +# Joint names in the target physics engine where policy is deployed (PhysX) +target_joint_names: + - "left_hip_yaw" + - "right_hip_yaw" + - "torso" + - "left_hip_roll" + - "right_hip_roll" + - "left_shoulder_pitch" + - "right_shoulder_pitch" + - "left_hip_pitch" + - "right_hip_pitch" + - "left_shoulder_roll" + - "right_shoulder_roll" + - "left_knee" + - "right_knee" + - "left_shoulder_yaw" + - "right_shoulder_yaw" + - "left_ankle" + - "right_ankle" + - "left_elbow" + - "right_elbow" diff --git a/scripts/sim2sim_transfer/rsl_rl_transfer.py b/scripts/sim2sim_transfer/rsl_rl_transfer.py new file mode 100644 index 00000000000..d35d57c6224 --- /dev/null +++ b/scripts/sim2sim_transfer/rsl_rl_transfer.py @@ -0,0 +1,286 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to play a checkpoint of an RL agent from RSL-RL with policy transfer capabilities.""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse +import os +import sys + +from isaaclab.app import AppLauncher + +# local imports +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../..")) +from scripts.reinforcement_learning.rsl_rl import cli_args # isort: skip + +# add argparse arguments +parser = argparse.ArgumentParser(description="Play an RL agent with RSL-RL with policy transfer.") +parser.add_argument("--video", action="store_true", default=False, help="Record videos during training.") +parser.add_argument("--video_length", type=int, default=200, help="Length of the recorded video (in steps).") +parser.add_argument( + "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +) +parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument( + "--agent", type=str, default="rsl_rl_cfg_entry_point", help="Name of the RL agent configuration entry point." +) +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") +parser.add_argument("--real-time", action="store_true", default=False, help="Run in real-time, if possible.") +# Joint ordering arguments +parser.add_argument( + "--policy_transfer_file", + type=str, + default=None, + help="Path to YAML file containing joint mapping configuration for policy transfer between physics engines.", +) +# append RSL-RL cli arguments +cli_args.add_rsl_rl_args(parser) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli, hydra_args = parser.parse_known_args() +# always enable cameras to record video +if args_cli.video: + args_cli.enable_cameras = True + +# clear out sys.argv for Hydra +sys.argv = [sys.argv[0]] + hydra_args + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import gymnasium as gym +import os +import time +import torch +import yaml + +from rsl_rl.runners import DistillationRunner, OnPolicyRunner + +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.dict import print_dict + +from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper, export_policy_as_jit, export_policy_as_onnx + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import get_checkpoint_path +from isaaclab_tasks.utils.hydra import hydra_task_config + +# PLACEHOLDER: Extension template (do not remove this comment) + + +def get_joint_mappings(args_cli, action_space_dim): + """Get joint mappings based on command line arguments. + + Args: + args_cli: Command line arguments + action_space_dim: Dimension of the action space (number of joints) + + Returns: + tuple: (source_to_target_list, target_to_source_list, source_to_target_obs_list) + """ + num_joints = action_space_dim + if args_cli.policy_transfer_file: + # Load from YAML file + try: + with open(args_cli.policy_transfer_file) as file: + config = yaml.safe_load(file) + except Exception as e: + raise RuntimeError(f"Failed to load joint mapping from {args_cli.policy_transfer_file}: {e}") + + source_joint_names = config["source_joint_names"] + target_joint_names = config["target_joint_names"] + # Find joint mapping + source_to_target = [] + target_to_source = [] + + # Create source to target mapping + for joint_name in source_joint_names: + if joint_name in target_joint_names: + source_to_target.append(target_joint_names.index(joint_name)) + else: + raise ValueError(f"Joint '{joint_name}' not found in target joint names") + + # Create target to source mapping + for joint_name in target_joint_names: + if joint_name in source_joint_names: + target_to_source.append(source_joint_names.index(joint_name)) + else: + raise ValueError(f"Joint '{joint_name}' not found in source joint names") + print(f"[INFO] Loaded joint mapping for policy transfer from YAML: {args_cli.policy_transfer_file}") + assert ( + len(source_to_target) == len(target_to_source) == num_joints + ), "Number of source and target joints must match" + else: + # Use identity mapping (one-to-one) + identity_map = list(range(num_joints)) + source_to_target, target_to_source = identity_map, identity_map + + # Create observation mapping (first 12 values stay the same for locomotion examples, then map joint-related values) + obs_map = ( + [0, 1, 2] + + [3, 4, 5] + + [6, 7, 8] + + [9, 10, 11] + + [i + 12 + num_joints * 0 for i in source_to_target] + + [i + 12 + num_joints * 1 for i in source_to_target] + + [i + 12 + num_joints * 2 for i in source_to_target] + ) + + return source_to_target, target_to_source, obs_map + + +@hydra_task_config(args_cli.task, args_cli.agent) +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg): + """Play with RSL-RL agent with policy transfer capabilities.""" + + # override configurations with non-hydra CLI arguments + agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) + env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs + + # set the environment seed + # note: certain randomizations occur in the environment initialization so we set the seed here + env_cfg.seed = agent_cfg.seed + env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + if args_cli.checkpoint: + resume_path = retrieve_file_path(args_cli.checkpoint) + else: + resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + log_dir = os.path.dirname(resume_path) + + # set the log directory for the environment (works for all environment types) + env_cfg.log_dir = log_dir + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + # wrap for video recording + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "play"), + "step_trigger": lambda step: step == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) + + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + if agent_cfg.class_name == "OnPolicyRunner": + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + elif agent_cfg.class_name == "DistillationRunner": + runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + else: + raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") + runner.load(resume_path) + + # obtain the trained policy for inference + policy = runner.get_inference_policy(device=env.unwrapped.device) + + # extract the neural network module + # we do this in a try-except to maintain backwards compatibility. + try: + # version 2.3 onwards + policy_nn = runner.alg.policy + except AttributeError: + # version 2.2 and below + policy_nn = runner.alg.actor_critic + + # extract the normalizer + if hasattr(policy_nn, "actor_obs_normalizer"): + normalizer = policy_nn.actor_obs_normalizer + elif hasattr(policy_nn, "student_obs_normalizer"): + normalizer = policy_nn.student_obs_normalizer + else: + normalizer = None + + # export policy to onnx/jit + export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") + export_policy_as_jit(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.pt") + export_policy_as_onnx(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.onnx") + + dt = env.unwrapped.step_dt + + # reset environment + obs = env.get_observations() + timestep = 0 + + # Get joint mappings for policy transfer + _, target_to_source, obs_map = get_joint_mappings(args_cli, env.action_space.shape[1]) + + # Create torch tensors for mappings + device = args_cli.device if args_cli.device else "cuda:0" + target_to_source_tensor = torch.tensor(target_to_source, device=device) if target_to_source else None + obs_map_tensor = torch.tensor(obs_map, device=device) if obs_map else None + + def remap_obs(obs): + """Remap the observation to the target observation space.""" + if obs_map_tensor is not None: + obs = obs[:, obs_map_tensor] + return obs + + def remap_actions(actions): + """Remap the actions to the target action space.""" + if target_to_source_tensor is not None: + actions = actions[:, target_to_source_tensor] + return actions + + # simulate environment + while simulation_app.is_running(): + start_time = time.time() + # run everything in inference mode + with torch.inference_mode(): + # agent stepping + actions = policy(remap_obs(obs)) + # env stepping + obs, _, _, _ = env.step(remap_actions(actions)) + if args_cli.video: + timestep += 1 + # Exit the play loop after recording one video + if timestep == args_cli.video_length: + break + + # time delay for real-time evaluation + sleep_time = dt - (time.time() - start_time) + if args_cli.real_time and sleep_time > 0: + time.sleep(sleep_time) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/tools/convert_mesh.py b/scripts/tools/convert_mesh.py index 50b295397d3..bce2c66ef71 100644 --- a/scripts/tools/convert_mesh.py +++ b/scripts/tools/convert_mesh.py @@ -43,6 +43,18 @@ from isaaclab.app import AppLauncher +# Define collision approximation choices (must be defined before parser) +_valid_collision_approx = [ + "convexDecomposition", + "convexHull", + "triangleMesh", + "meshSimplification", + "sdf", + "boundingCube", + "boundingSphere", + "none", +] + # add argparse arguments parser = argparse.ArgumentParser(description="Utility to convert a mesh file into USD format.") parser.add_argument("input", type=str, help="The path to the input mesh file.") @@ -57,11 +69,8 @@ "--collision-approximation", type=str, default="convexDecomposition", - choices=["convexDecomposition", "convexHull", "boundingCube", "boundingSphere", "meshSimplification", "none"], - help=( - 'The method used for approximating collision mesh. Set to "none" ' - "to not add a collision mesh to the converted mesh." - ), + choices=_valid_collision_approx, + help="The method used for approximating the collision mesh. Set to 'none' to disable collision mesh generation.", ) parser.add_argument( "--mass", @@ -92,6 +101,17 @@ from isaaclab.utils.assets import check_file_path from isaaclab.utils.dict import print_dict +collision_approximation_map = { + "convexDecomposition": schemas_cfg.ConvexDecompositionPropertiesCfg, + "convexHull": schemas_cfg.ConvexHullPropertiesCfg, + "triangleMesh": schemas_cfg.TriangleMeshPropertiesCfg, + "meshSimplification": schemas_cfg.TriangleMeshSimplificationPropertiesCfg, + "sdf": schemas_cfg.SDFMeshPropertiesCfg, + "boundingCube": schemas_cfg.BoundingCubePropertiesCfg, + "boundingSphere": schemas_cfg.BoundingSpherePropertiesCfg, + "none": None, +} + def main(): # check valid file path @@ -118,6 +138,15 @@ def main(): collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=args_cli.collision_approximation != "none") # Create Mesh converter config + cfg_class = collision_approximation_map.get(args_cli.collision_approximation) + if cfg_class is None and args_cli.collision_approximation != "none": + valid_keys = ", ".join(sorted(collision_approximation_map.keys())) + raise ValueError( + f"Invalid collision approximation type '{args_cli.collision_approximation}'. " + f"Valid options are: {valid_keys}." + ) + collision_cfg = cfg_class() if cfg_class is not None else None + mesh_converter_cfg = MeshConverterCfg( mass_props=mass_props, rigid_props=rigid_props, @@ -127,7 +156,7 @@ def main(): usd_dir=os.path.dirname(dest_path), usd_file_name=os.path.basename(dest_path), make_instanceable=args_cli.make_instanceable, - collision_approximation=args_cli.collision_approximation, + mesh_collision_props=collision_cfg, ) # Print info diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index ec01ffaaf8d..4eeef711a1c 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -33,7 +33,16 @@ # add argparse arguments parser = argparse.ArgumentParser(description="Record demonstrations for Isaac Lab environments.") parser.add_argument("--task", type=str, required=True, help="Name of the task.") -parser.add_argument("--teleop_device", type=str, default="keyboard", help="Device for interacting with environment.") +parser.add_argument( + "--teleop_device", + type=str, + default="keyboard", + help=( + "Teleop device. Set here (legacy) or via the environment config. If using the environment config, pass the" + " device key/name defined under 'teleop_devices' (it can be a custom name, not necessarily 'handtracking')." + " Built-ins: keyboard, spacemouse, gamepad. Not all tasks support all built-ins." + ), +) parser.add_argument( "--dataset_file", type=str, default="./datasets/dataset.hdf5", help="File path to export recorded demos." ) @@ -98,6 +107,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 from collections.abc import Callable @@ -304,7 +314,7 @@ def setup_ui(label_text: str, env: gym.Env) -> InstructionDisplay: Returns: InstructionDisplay: The configured instruction display object """ - instruction_display = InstructionDisplay(args_cli.teleop_device) + instruction_display = InstructionDisplay(args_cli.xr) if not args_cli.xr: window = EmptyWindow(env, "Instruction") with window.ui_window_elements["main_vstack"]: diff --git a/scripts/tools/replay_demos.py b/scripts/tools/replay_demos.py index 951220959b6..c23e3a10d87 100644 --- a/scripts/tools/replay_demos.py +++ b/scripts/tools/replay_demos.py @@ -66,6 +66,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg diff --git a/scripts/tutorials/01_assets/SPL_ball.py b/scripts/tutorials/01_assets/SPL_ball.py new file mode 100644 index 00000000000..c22604b3107 --- /dev/null +++ b/scripts/tutorials/01_assets/SPL_ball.py @@ -0,0 +1,173 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to work with the deformable object and interact with it. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p scripts/tutorials/01_assets/run_deformable_object.py + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="SPL BALL") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import isaacsim.core.utils.prims as prim_utils + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.assets import DeformableObject, DeformableObjectCfg +from isaaclab.sim import SimulationContext + + +def design_scene(): + """Designs the scene.""" + # Ground-plane + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + # Lights + cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.8, 0.8, 0.8)) + cfg.func("/World/Light", cfg) + + # Create separate groups called "Origin1", "Origin2", "Origin3" + # Each group will have a robot in it + origins = [[0.25, 0.25, 0.0], [-0.25, 0.25, 0.0], [0.25, -0.25, 0.0], [-0.25, -0.25, 0.0]] + for i, origin in enumerate(origins): + prim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin) + + # Deformable Object + cfg = DeformableObjectCfg( + prim_path="/World/Origin.*/Cube", + spawn=sim_utils.MeshCuboidCfg( + size=(0.05, 0.05, 0.05), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(rest_offset=0.0, contact_offset=0.001), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.2, 0.2)), + physics_material=sim_utils.DeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e4), + ), + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + debug_vis=True, + ) + cube_object = DeformableObject(cfg=cfg) + + # return the scene information + scene_entities = {"cube_object": cube_object} + return scene_entities, origins + + +def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, DeformableObject], origins: torch.Tensor): + """Runs the simulation loop.""" + # Extract scene entities + # note: we only do this here for readability. In general, it is better to access the entities directly from + # the dictionary. This dictionary is replaced by the InteractiveScene class in the next tutorial. + cube_object = entities["cube_object"] + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + # Nodal kinematic targets of the deformable bodies + nodal_kinematic_target = cube_object.data.nodal_kinematic_target.clone() + + # Simulate physics + while simulation_app.is_running(): + # reset + if count % 250 == 0: + # reset counters + sim_time = 0.0 + count = 0 + + # reset the nodal state of the object + nodal_state = cube_object.data.default_nodal_state_w.clone() + # apply random pose to the object + pos_w = torch.rand(cube_object.num_instances, 3, device=sim.device) * 0.1 + origins + quat_w = math_utils.random_orientation(cube_object.num_instances, device=sim.device) + nodal_state[..., :3] = cube_object.transform_nodal_pos(nodal_state[..., :3], pos_w, quat_w) + + # write nodal state to simulation + cube_object.write_nodal_state_to_sim(nodal_state) + + # Write the nodal state to the kinematic target and free all vertices + nodal_kinematic_target[..., :3] = nodal_state[..., :3] + nodal_kinematic_target[..., 3] = 1.0 + cube_object.write_nodal_kinematic_target_to_sim(nodal_kinematic_target) + + # reset buffers + cube_object.reset() + + print("----------------------------------------") + print("[INFO]: Resetting object state...") + + # update the kinematic target for cubes at index 0 and 3 + # we slightly move the cube in the z-direction by picking the vertex at index 0 + nodal_kinematic_target[[0, 3], 0, 2] += 0.001 + # set vertex at index 0 to be kinematically constrained + # 0: constrained, 1: free + nodal_kinematic_target[[0, 3], 0, 3] = 0.0 + # write kinematic target to simulation + cube_object.write_nodal_kinematic_target_to_sim(nodal_kinematic_target) + + # write internal data to simulation + cube_object.write_data_to_sim() + # perform step + sim.step() + # update sim-time + sim_time += sim_dt + count += 1 + # update buffers + cube_object.update(sim_dt) + # print the root position + if count % 50 == 0: + print(f"Root position (in world): {cube_object.data.root_pos_w[:, :3]}") + + +def main(): + """Main function.""" + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) + sim = SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view(eye=[3.0, 0.0, 1.0], target=[0.0, 0.0, 0.5]) + # Design scene + scene_entities, scene_origins = design_scene() + scene_origins = torch.tensor(scene_origins, device=sim.device) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene_entities, scene_origins) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index a53b7e970cb..cbc2de67560 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.45.15" +version = "0.47.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 91d5e1ab1ed..6e5ff947ae5 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,162 @@ Changelog --------- +0.47.2 (2025-10-22) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Fixed the data type conversion in :class:`~isaaclab.sensors.tiled_camera.TiledCamera` to + support the correct data type when converting from numpy arrays to warp arrays on the CPU. + + +0.47.1 (2025-10-17) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Suppressed yourdfpy warnings when trying to load meshes from hand urdfs in dex_retargeting. These mesh files are not + used by dex_retargeting, but the parser is incorrectly configured by dex_retargeting to load them anyway which results + in warning spam. + + +0.47.0 (2025-10-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Removed pickle utilities for saving and loading configurations as pickle contains security vulnerabilities in its APIs. + Configurations can continue to be saved and loaded through yaml. + + +0.46.11 (2025-10-15) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added support for modifying the :attr:`/rtx/domeLight/upperLowerStrategy` Sim rendering setting. + + +0.46.10 (2025-10-13) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ARM64 architecture for pink ik and dex-retargetting setup installations. + + +0.46.9 (2025-10-09) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.devices.keyboard.se3_keyboard.Se3Keyboard.__del__` to use the correct method name + for unsubscribing from keyboard events "unsubscribe_to_keyboard_events" instead of "unsubscribe_from_keyboard_events". + + +0.46.8 (2025-10-02) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed scaling factor for retargeting of GR1T2 hand. + + +0.46.7 (2025-09-30) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed finger joint indices with manus extension. + + +0.46.6 (2025-09-30) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added argument :attr:`traverse_instance_prims` to :meth:`~isaaclab.sim.utils.get_all_matching_child_prims` and + :meth:`~isaaclab.sim.utils.get_first_matching_child_prim` to control whether to traverse instance prims + during the traversal. Earlier, instanced prims were skipped since :meth:`Usd.Prim.GetChildren` did not return + instanced prims, which is now fixed. + +Changed +^^^^^^^ + +* Made parsing of instanced prims in :meth:`~isaaclab.sim.utils.get_all_matching_child_prims` and + :meth:`~isaaclab.sim.utils.get_first_matching_child_prim` as the default behavior. +* Added parsing of instanced prims in :meth:`~isaaclab.sim.utils.make_uninstanceable` to make all prims uninstanceable. + + +0.46.5 (2025-10-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Exposed parameter :attr:`~isaaclab.sim.spawners.PhysxCfg.solve_articulation_contact_last` + to configure USD attribute ``physxscene:solveArticulationContactLast``. This parameter may + help improve solver stability with grippers, which previously required reducing simulation time-steps. + :class:`~isaaclab.sim.spawners.PhysxCfg` + + +0.46.4 (2025-10-06) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Fixed :attr:`~isaaclab.sim.simulation_context.SimulationContext.device` to return the device from the configuration. + Previously, it was returning the device from the simulation manager, which was causing a performance overhead. + + +0.46.3 (2025-09-17) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Modified setter to support for viscous and dynamic joint friction coefficients in articulation based on IsaacSim 5.0. +* Added randomization of viscous and dynamic joint friction coefficients in event term. + + +0.46.2 (2025-09-13) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Fixed missing actuator indices in :meth:`~isaaclab.envs.mdp.events.randomize_actuator_gains` + + +0.46.1 (2025-09-10) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Moved IO descriptors output directory to a subfolder under the task log directory. + + +0.46.0 (2025-09-06) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added teleoperation environments for Unitree G1. This includes an environment with lower body fixed and upper body + controlled by IK, and an environment with the lower body controlled by a policy and the upper body controlled by IK. + + 0.45.15 (2025-09-05) ~~~~~~~~~~~~~~~~~~~~ @@ -4949,8 +5105,7 @@ Added ~~~~~~~~~~~~~~~~~~ * Added the :class:`isaaclab.app.AppLauncher` class to allow controlled instantiation of - the `SimulationApp `_ - and extension loading for remote deployment and ROS bridges. + the SimulationApp and extension loading for remote deployment and ROS bridges. Changed ^^^^^^^ diff --git a/source/isaaclab/isaaclab/actuators/actuator_pd.py b/source/isaaclab/isaaclab/actuators/actuator_pd.py index 11c39f20177..162005dfd17 100644 --- a/source/isaaclab/isaaclab/actuators/actuator_pd.py +++ b/source/isaaclab/isaaclab/actuators/actuator_pd.py @@ -152,7 +152,7 @@ class IdealPDActuator(ActuatorBase): .. math:: - \tau_{j, computed} = k_p * (q - q_{des}) + k_d * (\dot{q} - \dot{q}_{des}) + \tau_{ff} + \tau_{j, computed} = k_p * (q_{des} - q) + k_d * (\dot{q}_{des} - \dot{q}) + \tau_{ff} where, :math:`k_p` and :math:`k_d` are joint stiffness and damping gains, :math:`q` and :math:`\dot{q}` are the current joint positions and velocities, :math:`q_{des}`, :math:`\dot{q}_{des}` and :math:`\tau_{ff}` diff --git a/source/isaaclab/isaaclab/app/app_launcher.py b/source/isaaclab/isaaclab/app/app_launcher.py index aa97e2bc3ec..1ecfbdd8642 100644 --- a/source/isaaclab/isaaclab/app/app_launcher.py +++ b/source/isaaclab/isaaclab/app/app_launcher.py @@ -76,7 +76,7 @@ def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwa such as ``LIVESTREAM``. .. _argparse.Namespace: https://docs.python.org/3/library/argparse.html?highlight=namespace#argparse.Namespace - .. _SimulationApp: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.simulation_app/docs/index.html + .. _SimulationApp: https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.simulation_app/docs/index.html#isaacsim.simulation_app.SimulationApp """ # We allow users to pass either a dict or an argparse.Namespace into # __init__, anticipating that these will be all of the argparse arguments diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation.py b/source/isaaclab/isaaclab/assets/articulation/articulation.py index 55f3e650ed0..d09eaadb403 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation.py @@ -827,24 +827,33 @@ def write_joint_armature_to_sim( def write_joint_friction_coefficient_to_sim( self, joint_friction_coeff: torch.Tensor | float, + joint_dynamic_friction_coeff: torch.Tensor | float | None = None, + joint_viscous_friction_coeff: torch.Tensor | float | None = None, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None, ): - r"""Write joint static friction coefficients into the simulation. + r"""Write joint friction coefficients into the simulation. - The joint static friction is a unitless quantity. It relates the magnitude of the spatial force transmitted - from the parent body to the child body to the maximal static friction force that may be applied by the solver - to resist the joint motion. + For Isaac Sim versions below 5.0, only the static friction coefficient is set. + This limits the resisting force or torque up to a maximum proportional to the transmitted + spatial force: :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`. - Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` - is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force - transmitted from the parent body to the child body. The simulated static friction effect is therefore - similar to static and Coulomb static friction. + For Isaac Sim versions 5.0 and above, the static, dynamic, and viscous friction coefficients + are set. The model combines Coulomb (static & dynamic) friction with a viscous term: + + - Static friction :math:`\mu_s` defines the maximum effort that prevents motion at rest. + - Dynamic friction :math:`\mu_d` applies once motion begins and remains constant during motion. + - Viscous friction :math:`c_v` is a velocity-proportional resistive term. Args: - joint_friction_coeff: Joint static friction coefficient. Shape is (len(env_ids), len(joint_ids)). - joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). - env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). + joint_friction_coeff: Static friction coefficient :math:`\mu_s`. + Shape is (len(env_ids), len(joint_ids)). Scalars are broadcast to all selections. + joint_dynamic_friction_coeff: Dynamic (Coulomb) friction coefficient :math:`\mu_d`. + Same shape as above. If None, the dynamic coefficient is not updated. + joint_viscous_friction_coeff: Viscous friction coefficient :math:`c_v`. + Same shape as above. If None, the viscous coefficient is not updated. + joint_ids: The joint indices to set the friction coefficients for. Defaults to None (all joints). + env_ids: The environment indices to set the friction coefficients for. Defaults to None (all environments). """ # resolve indices physx_env_ids = env_ids @@ -858,15 +867,38 @@ def write_joint_friction_coefficient_to_sim( env_ids = env_ids[:, None] # set into internal buffers self._data.joint_friction_coeff[env_ids, joint_ids] = joint_friction_coeff + + # if dynamic or viscous friction coeffs are provided, set them too + if joint_dynamic_friction_coeff is not None: + self._data.joint_dynamic_friction_coeff[env_ids, joint_ids] = joint_dynamic_friction_coeff + if joint_viscous_friction_coeff is not None: + self._data.joint_viscous_friction_coeff[env_ids, joint_ids] = joint_viscous_friction_coeff + + # move the indices to cpu + physx_envs_ids_cpu = physx_env_ids.cpu() + # set into simulation if int(get_version()[2]) < 5: self.root_physx_view.set_dof_friction_coefficients( - self._data.joint_friction_coeff.cpu(), indices=physx_env_ids.cpu() + self._data.joint_friction_coeff.cpu(), indices=physx_envs_ids_cpu ) else: friction_props = self.root_physx_view.get_dof_friction_properties() - friction_props[physx_env_ids.cpu(), :, 0] = self._data.joint_friction_coeff[physx_env_ids, :].cpu() - self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_env_ids.cpu()) + friction_props[physx_envs_ids_cpu, :, 0] = self._data.joint_friction_coeff[physx_envs_ids_cpu, :].cpu() + + # only set dynamic and viscous friction if provided + if joint_dynamic_friction_coeff is not None: + friction_props[physx_envs_ids_cpu, :, 1] = self._data.joint_dynamic_friction_coeff[ + physx_envs_ids_cpu, : + ].cpu() + + # only set viscous friction if provided + if joint_viscous_friction_coeff is not None: + friction_props[physx_envs_ids_cpu, :, 2] = self._data.joint_viscous_friction_coeff[ + physx_envs_ids_cpu, : + ].cpu() + + self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_envs_ids_cpu) def write_joint_dynamic_friction_coefficient_to_sim( self, diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index 145a69dfc85..99b2f76abfa 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -151,8 +151,9 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default inertia for all the bodies in the articulation. Shape is (num_instances, num_bodies, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the articulation links' actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. This quantity is parsed from the USD schema at the time of initialization. """ diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py index 3aac87d324f..ee83900376f 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py @@ -112,8 +112,11 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default inertia tensor read from the simulation. Shape is (num_instances, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the rigid body's actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. + + This quantity is parsed from the USD schema at the time of initialization. """ ## diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py index 897679f75aa..328010bb14f 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py @@ -118,8 +118,11 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default object inertia tensor read from the simulation. Shape is (num_instances, num_objects, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the rigid body's actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. + + This quantity is parsed from the USD schema at the time of initialization. """ ## diff --git a/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py index 1702dbf90e2..fc56608d4e7 100644 --- a/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py +++ b/source/isaaclab/isaaclab/assets/surface_gripper/surface_gripper.py @@ -162,9 +162,9 @@ def update(self, dt: float) -> None: This function is called every simulation step. The data fetched from the gripper view is a list of strings containing 3 possible states: - - "Open" - - "Closing" - - "Closed" + - "Open" --> 0 + - "Closing" --> 1 + - "Closed" --> 2 To make this more neural network friendly, we convert the list of strings to a list of floats: - "Open" --> -1.0 @@ -175,11 +175,8 @@ def update(self, dt: float) -> None: We need to do this conversion for every single step of the simulation because the gripper can lose contact with the object if some conditions are met: such as if a large force is applied to the gripped object. """ - state_list: list[str] = self._gripper_view.get_surface_gripper_status() - state_list_as_int: list[float] = [ - -1.0 if state == "Open" else 1.0 if state == "Closed" else 0.0 for state in state_list - ] - self._gripper_state = torch.tensor(state_list_as_int, dtype=torch.float32, device=self._device) + state_list: list[int] = self._gripper_view.get_surface_gripper_status() + self._gripper_state = torch.tensor(state_list, dtype=torch.float32, device=self._device) - 1.0 def write_data_to_sim(self) -> None: """Write the gripper command to the SurfaceGripperView. diff --git a/source/isaaclab/isaaclab/controllers/config/rmp_flow.py b/source/isaaclab/isaaclab/controllers/config/rmp_flow.py index e1b18350e14..f3d214168fb 100644 --- a/source/isaaclab/isaaclab/controllers/config/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/config/rmp_flow.py @@ -72,3 +72,23 @@ ) """Configuration of RMPFlow for Galbot humanoid.""" + +AGIBOT_LEFT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_left_arm_rmpflow_config.yaml"), + urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "agibot.urdf"), + collision_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_left_arm_gripper.yaml"), + frame_name="gripper_center", + evaluations_per_frame=5, + ignore_robot_state_updates=True, +) + +AGIBOT_RIGHT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg( + config_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_right_arm_rmpflow_config.yaml"), + urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "agibot.urdf"), + collision_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_right_arm_gripper.yaml"), + frame_name="right_gripper_center", + evaluations_per_frame=5, + ignore_robot_state_updates=True, +) + +"""Configuration of RMPFlow for Agibot humanoid.""" diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py new file mode 100644 index 00000000000..e46174bcaa5 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2025, 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 numpy as np +from collections.abc import Sequence + +import pinocchio as pin +from pink.tasks.frame_task import FrameTask + +from .pink_kinematics_configuration import PinkKinematicsConfiguration + + +class LocalFrameTask(FrameTask): + """ + A task that computes error in a local (custom) frame. + Inherits from FrameTask but overrides compute_error. + """ + + def __init__( + self, + frame: str, + base_link_frame_name: str, + position_cost: float | Sequence[float], + orientation_cost: float | Sequence[float], + lm_damping: float = 0.0, + gain: float = 1.0, + ): + """ + Initialize the LocalFrameTask with configuration. + + This task computes pose errors in a local (custom) frame rather than the world frame, + allowing for more flexible control strategies where the reference frame can be + specified independently. + + Args: + frame: Name of the frame to control (end-effector or target frame). + base_link_frame_name: Name of the base link frame used as reference frame + for computing transforms and errors. + position_cost: Cost weight(s) for position error. Can be a single float + for uniform weighting or a sequence of 3 floats for per-axis weighting. + orientation_cost: Cost weight(s) for orientation error. Can be a single float + for uniform weighting or a sequence of 3 floats for per-axis weighting. + lm_damping: Levenberg-Marquardt damping factor for numerical stability. + Defaults to 0.0 (no damping). + gain: Task gain factor that scales the overall task contribution. + Defaults to 1.0. + """ + super().__init__(frame, position_cost, orientation_cost, lm_damping, gain) + self.base_link_frame_name = base_link_frame_name + self.transform_target_to_base = None + + def set_target(self, transform_target_to_base: pin.SE3) -> None: + """Set task target pose in the world frame. + + Args: + transform_target_to_world: Transform from the task target frame to + the world frame. + """ + self.transform_target_to_base = transform_target_to_base.copy() + + def set_target_from_configuration(self, configuration: PinkKinematicsConfiguration) -> None: + """Set task target pose from a robot configuration. + + Args: + configuration: Robot configuration. + """ + if not isinstance(configuration, PinkKinematicsConfiguration): + raise ValueError("configuration must be a PinkKinematicsConfiguration") + self.set_target(configuration.get_transform(self.frame, self.base_link_frame_name)) + + def compute_error(self, configuration: PinkKinematicsConfiguration) -> np.ndarray: + """ + Compute the error between current and target pose in a local frame. + """ + if not isinstance(configuration, PinkKinematicsConfiguration): + raise ValueError("configuration must be a PinkKinematicsConfiguration") + if self.transform_target_to_base is None: + raise ValueError(f"no target set for frame '{self.frame}'") + + transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name) + transform_target_to_frame = transform_frame_to_base.actInv(self.transform_target_to_base) + + error_in_frame: np.ndarray = pin.log(transform_target_to_frame).vector + return error_in_frame + + def compute_jacobian(self, configuration: PinkKinematicsConfiguration) -> np.ndarray: + r"""Compute the frame task Jacobian. + + The task Jacobian :math:`J(q) \in \mathbb{R}^{6 \times n_v}` is the + derivative of the task error :math:`e(q) \in \mathbb{R}^6` with respect + to the configuration :math:`q`. The formula for the frame task is: + + .. math:: + + J(q) = -\text{Jlog}_6(T_{tb}) {}_b J_{0b}(q) + + The derivation of the formula for this Jacobian is detailed in + [Caron2023]_. See also + :func:`pink.tasks.task.Task.compute_jacobian` for more context on task + Jacobians. + + Args: + configuration: Robot configuration :math:`q`. + + Returns: + Jacobian matrix :math:`J`, expressed locally in the frame. + """ + if self.transform_target_to_base is None: + raise Exception(f"no target set for frame '{self.frame}'") + transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name) + transform_frame_to_target = self.transform_target_to_base.actInv(transform_frame_to_base) + jacobian_in_frame = configuration.get_frame_jacobian(self.frame) + J = -pin.Jlog6(transform_frame_to_target) @ jacobian_in_frame + return J diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index 6bb4228e4e8..344244d141b 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -19,14 +19,12 @@ from typing import TYPE_CHECKING from pink import solve_ik -from pink.configuration import Configuration -from pink.tasks import FrameTask -from pinocchio.robot_wrapper import RobotWrapper from isaaclab.assets import ArticulationCfg from isaaclab.utils.string import resolve_matching_names_values from .null_space_posture_task import NullSpacePostureTask +from .pink_kinematics_configuration import PinkKinematicsConfiguration if TYPE_CHECKING: from .pink_ik_cfg import PinkIKControllerCfg @@ -47,7 +45,9 @@ class PinkIKController: Pink IK Solver: https://github.com/stephane-caron/pink """ - def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str): + def __init__( + self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str, controlled_joint_indices: list[int] + ): """Initialize the Pink IK Controller. Args: @@ -56,14 +56,28 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: robot_cfg: The robot articulation configuration containing initial joint positions and robot specifications. device: The device to use for computations (e.g., 'cuda:0', 'cpu'). + controlled_joint_indices: A list of joint indices in the USD asset controlled by the Pink IK controller. Raises: - KeyError: When Pink joint names cannot be matched to robot configuration joint positions. + ValueError: When joint_names or all_joint_names are not provided in the configuration. """ - # Initialize the robot model from URDF and mesh files - self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None) - self.pink_configuration = Configuration( - self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0 + if cfg.joint_names is None: + raise ValueError("joint_names must be provided in the configuration") + if cfg.all_joint_names is None: + raise ValueError("all_joint_names must be provided in the configuration") + + self.cfg = cfg + self.device = device + self.controlled_joint_indices = controlled_joint_indices + + # Validate consistency between controlled_joint_indices and configuration + self._validate_consistency(cfg, controlled_joint_indices) + + # Initialize the Kinematics model used by pink IK to control robot + self.pink_configuration = PinkKinematicsConfiguration( + urdf_path=cfg.urdf_path, + mesh_path=cfg.mesh_path, + controlled_joint_names=cfg.joint_names, ) # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos, @@ -73,16 +87,11 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: joint_pos_dict = robot_cfg.init_state.joint_pos # Use resolve_matching_names_values to match Pink joint names to joint_pos values - indices, names, values = resolve_matching_names_values( + indices, _, values = resolve_matching_names_values( joint_pos_dict, pink_joint_names, preserve_order=False, strict=False ) - if len(indices) != len(pink_joint_names): - unmatched = [name for name in pink_joint_names if name not in names] - raise KeyError( - "Could not find a match for all Pink joint names in robot_cfg.init_state.joint_pos. " - f"Unmatched: {unmatched}, Expected: {pink_joint_names}" - ) - self.init_joint_positions = np.array(values) + self.init_joint_positions = np.zeros(len(pink_joint_names)) + self.init_joint_positions[indices] = np.array(values) # Set the default targets for each task from the configuration for task in cfg.variable_input_tasks: @@ -94,27 +103,75 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: for task in cfg.fixed_input_tasks: task.set_target_from_configuration(self.pink_configuration) - # Map joint names from Isaac Lab to Pink's joint conventions - self.pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints - self.isaac_lab_joint_names = cfg.joint_names - assert cfg.joint_names is not None, "cfg.joint_names cannot be None" + # Create joint ordering mappings + self._setup_joint_ordering_mappings() - # Frame task link names - self.frame_task_link_names = [] - for task in cfg.variable_input_tasks: - if isinstance(task, FrameTask): - self.frame_task_link_names.append(task.frame) + def _validate_consistency(self, cfg: PinkIKControllerCfg, controlled_joint_indices: list[int]) -> None: + """Validate consistency between controlled_joint_indices and controller configuration. + + Args: + cfg: The Pink IK controller configuration. + controlled_joint_indices: List of joint indices in Isaac Lab joint space. + + Raises: + ValueError: If any consistency checks fail. + """ + # Check: Length consistency + if cfg.joint_names is None: + raise ValueError("cfg.joint_names cannot be None") + if len(controlled_joint_indices) != len(cfg.joint_names): + raise ValueError( + f"Length mismatch: controlled_joint_indices has {len(controlled_joint_indices)} elements " + f"but cfg.joint_names has {len(cfg.joint_names)} elements" + ) + + # Check: Joint name consistency - verify that the indices point to the expected joint names + actual_joint_names = [cfg.all_joint_names[idx] for idx in controlled_joint_indices] + if actual_joint_names != cfg.joint_names: + mismatches = [] + for i, (actual, expected) in enumerate(zip(actual_joint_names, cfg.joint_names)): + if actual != expected: + mismatches.append( + f"Index {i}: index {controlled_joint_indices[i]} points to '{actual}' but expected '{expected}'" + ) + if mismatches: + raise ValueError( + "Joint name mismatch between controlled_joint_indices and cfg.joint_names:\n" + + "\n".join(mismatches) + ) - # Create reordering arrays for joint indices + def _setup_joint_ordering_mappings(self): + """Setup joint ordering mappings between Isaac Lab and Pink conventions.""" + pink_joint_names = self.pink_configuration.all_joint_names_pinocchio_order + isaac_lab_joint_names = self.cfg.all_joint_names + + if pink_joint_names is None: + raise ValueError("pink_joint_names should not be None") + if isaac_lab_joint_names is None: + raise ValueError("isaac_lab_joint_names should not be None") + + # Create reordering arrays for all joints self.isaac_lab_to_pink_ordering = np.array( - [self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names] + [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names] ) self.pink_to_isaac_lab_ordering = np.array( - [self.pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in self.isaac_lab_joint_names] + [pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names] ) + # Create reordering arrays for controlled joints only + pink_controlled_joint_names = self.pink_configuration.controlled_joint_names_pinocchio_order + isaac_lab_controlled_joint_names = self.cfg.joint_names - self.cfg = cfg - self.device = device + if pink_controlled_joint_names is None: + raise ValueError("pink_controlled_joint_names should not be None") + if isaac_lab_controlled_joint_names is None: + raise ValueError("isaac_lab_controlled_joint_names should not be None") + + self.isaac_lab_to_pink_controlled_ordering = np.array( + [isaac_lab_controlled_joint_names.index(pink_joint) for pink_joint in pink_controlled_joint_names] + ) + self.pink_to_isaac_lab_controlled_ordering = np.array( + [pink_controlled_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_controlled_joint_names] + ) def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): """Update the null space joint targets. @@ -149,13 +206,16 @@ def compute( The target joint positions as a tensor of shape (num_joints,) on the specified device. If the IK solver fails, returns the current joint positions unchanged to maintain stability. """ + # Get the current controlled joint positions + curr_controlled_joint_pos = [curr_joint_pos[i] for i in self.controlled_joint_indices] + # Initialize joint positions for Pink, change from isaac_lab to pink/pinocchio joint ordering. joint_positions_pink = curr_joint_pos[self.isaac_lab_to_pink_ordering] # Update Pink's robot configuration with the current joint positions self.pink_configuration.update(joint_positions_pink) - # pink.solve_ik can raise an exception if the solver fails + # Solve IK using Pink's solver try: velocity = solve_ik( self.pink_configuration, @@ -164,7 +224,7 @@ def compute( solver="osqp", safety_break=self.cfg.fail_on_joint_limit_violation, ) - Delta_q = velocity * dt + joint_angle_changes = velocity * dt except (AssertionError, Exception) as e: # Print warning and return the current joint positions as the target # Not using omni.log since its not available in CI during docs build @@ -178,21 +238,18 @@ def compute( from isaaclab.ui.xr_widgets import XRVisualization XRVisualization.push_event("ik_error", {"error": e}) - return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) - - # Discard the first 6 values (for root and universal joints) - pink_joint_angle_changes = Delta_q + return torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32) # Reorder the joint angle changes back to Isaac Lab conventions joint_vel_isaac_lab = torch.tensor( - pink_joint_angle_changes[self.pink_to_isaac_lab_ordering], + joint_angle_changes[self.pink_to_isaac_lab_controlled_ordering], device=self.device, - dtype=torch.float, + dtype=torch.float32, ) # Add the velocity changes to the current joint positions to get the target joint positions target_joint_pos = torch.add( - joint_vel_isaac_lab, torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) + joint_vel_isaac_lab, torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32) ) return target_joint_pos diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py index d5f36a91523..ed7e40b0c48 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py @@ -46,6 +46,10 @@ class PinkIKControllerCfg: """ joint_names: list[str] | None = None + """A list of joint names in the USD asset controlled by the Pink IK controller. This is required because the joint naming conventions differ between USD and URDF files. + This value is currently designed to be automatically populated by the action term in a manager based environment.""" + + all_joint_names: list[str] | None = None """A list of joint names in the USD asset. This is required because the joint naming conventions differ between USD and URDF files. This value is currently designed to be automatically populated by the action term in a manager based environment.""" diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py new file mode 100644 index 00000000000..6bc11c5f198 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py @@ -0,0 +1,181 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np + +import pinocchio as pin +from pink.configuration import Configuration +from pink.exceptions import FrameNotFound +from pinocchio.robot_wrapper import RobotWrapper + + +class PinkKinematicsConfiguration(Configuration): + """ + A configuration class that maintains both a "controlled" (reduced) model and a "full" model. + + This class extends the standard Pink Configuration to allow for selective joint control: + - The "controlled" model/data/q represent the subset of joints being actively controlled (e.g., a kinematic chain or arm). + - The "full" model/data/q represent the complete robot, including all joints. + + This is useful for scenarios where only a subset of joints are being optimized or controlled, but full-model kinematics + (e.g., for collision checking, full-body Jacobians, or visualization) are still required. + + The class ensures that both models are kept up to date, and provides methods to update both the controlled and full + configurations as needed. + """ + + def __init__( + self, + controlled_joint_names: list[str], + urdf_path: str, + mesh_path: str | None = None, + copy_data: bool = True, + forward_kinematics: bool = True, + ): + """ + Initialize PinkKinematicsConfiguration. + + Args: + urdf_path (str): Path to the robot URDF file. + mesh_path (str): Path to the mesh files for the robot. + controlled_joint_names (list[str]): List of joint names to be actively controlled. + copy_data (bool, optional): If True, work on an internal copy of the input data. Defaults to True. + forward_kinematics (bool, optional): If True, compute forward kinematics from the configuration vector. Defaults to True. + + This constructor initializes the PinkKinematicsConfiguration, which maintains both a "controlled" (reduced) model and a "full" model. + The controlled model/data/q represent the subset of joints being actively controlled, while the full model/data/q represent the complete robot. + This is useful for scenarios where only a subset of joints are being optimized or controlled, but full-model kinematics are still required. + """ + self._controlled_joint_names = controlled_joint_names + + # Build robot model with all joints + if mesh_path: + self.robot_wrapper = RobotWrapper.BuildFromURDF(urdf_path, mesh_path) + else: + self.robot_wrapper = RobotWrapper.BuildFromURDF(urdf_path) + self.full_model = self.robot_wrapper.model + self.full_data = self.robot_wrapper.data + self.full_q = self.robot_wrapper.q0 + + # import pdb; pdb.set_trace() + self._all_joint_names = self.full_model.names.tolist()[1:] + # controlled_joint_indices: indices in all_joint_names for joints that are in controlled_joint_names, preserving all_joint_names order + self._controlled_joint_indices = [ + idx for idx, joint_name in enumerate(self._all_joint_names) if joint_name in self._controlled_joint_names + ] + + # Build the reduced model with only the controlled joints + joints_to_lock = [] + for joint_name in self._all_joint_names: + if joint_name not in self._controlled_joint_names: + joints_to_lock.append(self.full_model.getJointId(joint_name)) + + if len(joints_to_lock) == 0: + # No joints to lock, controlled model is the same as full model + self.controlled_model = self.full_model + self.controlled_data = self.full_data + self.controlled_q = self.full_q + else: + self.controlled_model = pin.buildReducedModel(self.full_model, joints_to_lock, self.full_q) + self.controlled_data = self.controlled_model.createData() + self.controlled_q = self.full_q[self._controlled_joint_indices] + + # Pink will should only have the controlled model + super().__init__(self.controlled_model, self.controlled_data, self.controlled_q, copy_data, forward_kinematics) + + def update(self, q: np.ndarray | None = None) -> None: + """Update configuration to a new vector. + + Calling this function runs forward kinematics and computes + collision-pair distances, if applicable. + + Args: + q: New configuration vector. + """ + if q is not None and len(q) != len(self._all_joint_names): + raise ValueError("q must have the same length as the number of joints in the model") + if q is not None: + super().update(q[self._controlled_joint_indices]) + + q_readonly = q.copy() + q_readonly.setflags(write=False) + self.full_q = q_readonly + pin.computeJointJacobians(self.full_model, self.full_data, q) + pin.updateFramePlacements(self.full_model, self.full_data) + else: + super().update() + pin.computeJointJacobians(self.full_model, self.full_data, self.full_q) + pin.updateFramePlacements(self.full_model, self.full_data) + + def get_frame_jacobian(self, frame: str) -> np.ndarray: + r"""Compute the Jacobian matrix of a frame velocity. + + Denoting our frame by :math:`B` and the world frame by :math:`W`, the + Jacobian matrix :math:`{}_B J_{WB}` is related to the body velocity + :math:`{}_B v_{WB}` by: + + .. math:: + + {}_B v_{WB} = {}_B J_{WB} \dot{q} + + Args: + frame: Name of the frame, typically a link name from the URDF. + + Returns: + Jacobian :math:`{}_B J_{WB}` of the frame. + + When the robot model includes a floating base + (pin.JointModelFreeFlyer), the configuration vector :math:`q` consists + of: + + - ``q[0:3]``: position in [m] of the floating base in the inertial + frame, formatted as :math:`[p_x, p_y, p_z]`. + - ``q[3:7]``: unit quaternion for the orientation of the floating base + in the inertial frame, formatted as :math:`[q_x, q_y, q_z, q_w]`. + - ``q[7:]``: joint angles in [rad]. + """ + if not self.full_model.existFrame(frame): + raise FrameNotFound(frame, self.full_model.frames) + frame_id = self.full_model.getFrameId(frame) + J: np.ndarray = pin.getFrameJacobian(self.full_model, self.full_data, frame_id, pin.ReferenceFrame.LOCAL) + return J[:, self._controlled_joint_indices] + + def get_transform_frame_to_world(self, frame: str) -> pin.SE3: + """Get the pose of a frame in the current configuration. + We override this method from the super class to solve the issue that in the default + Pink implementation, the frame placements do not take into account the non-controlled joints + being not at initial pose (which is a bad assumption when they are controlled by other controllers like a lower body controller). + + Args: + frame: Name of a frame, typically a link name from the URDF. + + Returns: + Current transform from the given frame to the world frame. + + Raises: + FrameNotFound: if the frame name is not found in the robot model. + """ + frame_id = self.full_model.getFrameId(frame) + try: + return self.full_data.oMf[frame_id].copy() + except IndexError as index_error: + raise FrameNotFound(frame, self.full_model.frames) from index_error + + def check_limits(self, tol: float = 1e-6, safety_break: bool = True) -> None: + """Check if limits are violated only if safety_break is enabled""" + if safety_break: + super().check_limits(tol, safety_break) + + @property + def controlled_joint_names_pinocchio_order(self) -> list[str]: + """Get the names of the controlled joints in the order of the pinocchio model.""" + return [self._all_joint_names[i] for i in self._controlled_joint_indices] + + @property + def all_joint_names_pinocchio_order(self) -> list[str]: + """Get the names of all joints in the order of the pinocchio model.""" + return self._all_joint_names diff --git a/source/isaaclab/isaaclab/controllers/utils.py b/source/isaaclab/isaaclab/controllers/utils.py index 70d627ac201..b674b267acb 100644 --- a/source/isaaclab/isaaclab/controllers/utils.py +++ b/source/isaaclab/isaaclab/controllers/utils.py @@ -9,6 +9,7 @@ """ import os +import re from isaacsim.core.utils.extensions import enable_extension @@ -98,3 +99,38 @@ def change_revolute_to_fixed(urdf_path: str, fixed_joints: list[str], verbose: b with open(urdf_path, "w") as file: file.write(content) + + +def change_revolute_to_fixed_regex(urdf_path: str, fixed_joints: list[str], verbose: bool = False): + """Change revolute joints to fixed joints in a URDF file. + + This function modifies a URDF file by changing specified revolute joints to fixed joints. + This is useful when you want to disable certain joints in a robot model. + + Args: + urdf_path: Path to the URDF file to modify. + fixed_joints: List of regular expressions matching joint names to convert from revolute to fixed. + verbose: Whether to print information about the changes being made. + """ + + with open(urdf_path) as file: + content = file.read() + + # Find all revolute joints in the URDF + revolute_joints = re.findall(r'', content) + + for joint in revolute_joints: + # Check if this joint matches any of the fixed joint patterns + should_fix = any(re.match(pattern, joint) for pattern in fixed_joints) + + if should_fix: + old_str = f'' + new_str = f'' + if verbose: + omni.log.warn(f"Replacing {joint} with fixed joint") + omni.log.warn(old_str) + omni.log.warn(new_str) + content = content.replace(old_str, new_str) + + with open(urdf_path, "w") as file: + file.write(content) diff --git a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py index 53682c12428..45edf1145b7 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py @@ -82,7 +82,7 @@ def __init__(self, cfg: Se2KeyboardCfg): def __del__(self): """Release the keyboard interface.""" - self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub) + self._input.unsubscribe_to_keyboard_events(self._keyboard, self._keyboard_sub) self._keyboard_sub = None def __str__(self) -> str: diff --git a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py index 64e398ad14e..94b654e8b17 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py @@ -90,7 +90,7 @@ def __init__(self, cfg: Se3KeyboardCfg): def __del__(self): """Release the keyboard interface.""" - self._input.unsubscribe_from_keyboard_events(self._keyboard, self._keyboard_sub) + self._input.unsubscribe_to_keyboard_events(self._keyboard, self._keyboard_sub) self._keyboard_sub = None def __str__(self) -> str: diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py index c58e32fa0d2..3044579136e 100644 --- a/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py @@ -7,7 +7,7 @@ import numpy as np from time import time -import carb +import omni.log from isaacsim.core.utils.extensions import enable_extension # For testing purposes, we need to mock the XRCore @@ -20,39 +20,39 @@ # Mapping from Manus joint index (0-24) to joint name. Palm (25) is calculated from middle metacarpal and proximal. HAND_JOINT_MAP = { - # Palm - 25: "palm", # Wrist 0: "wrist", # Thumb - 21: "thumb_metacarpal", - 22: "thumb_proximal", - 23: "thumb_distal", - 24: "thumb_tip", + 1: "thumb_metacarpal", + 2: "thumb_proximal", + 3: "thumb_distal", + 4: "thumb_tip", # Index - 1: "index_metacarpal", - 2: "index_proximal", - 3: "index_intermediate", - 4: "index_distal", - 5: "index_tip", + 5: "index_metacarpal", + 6: "index_proximal", + 7: "index_intermediate", + 8: "index_distal", + 9: "index_tip", # Middle - 6: "middle_metacarpal", - 7: "middle_proximal", - 8: "middle_intermediate", - 9: "middle_distal", - 10: "middle_tip", + 10: "middle_metacarpal", + 11: "middle_proximal", + 12: "middle_intermediate", + 13: "middle_distal", + 14: "middle_tip", # Ring - 11: "ring_metacarpal", - 12: "ring_proximal", - 13: "ring_intermediate", - 14: "ring_distal", - 15: "ring_tip", + 15: "ring_metacarpal", + 16: "ring_proximal", + 17: "ring_intermediate", + 18: "ring_distal", + 19: "ring_tip", # Little - 16: "little_metacarpal", - 17: "little_proximal", - 18: "little_intermediate", - 19: "little_distal", - 20: "little_tip", + 20: "little_metacarpal", + 21: "little_proximal", + 22: "little_intermediate", + 23: "little_distal", + 24: "little_tip", + # Palm + 25: "palm", } @@ -144,7 +144,7 @@ def update_vive(self): if self.scene_T_lighthouse_static is None: self._initialize_coordinate_transformation() except Exception as e: - carb.log_error(f"Vive tracker update failed: {e}") + omni.log.error(f"Vive tracker update failed: {e}") def _initialize_coordinate_transformation(self): """ @@ -214,8 +214,12 @@ def _initialize_coordinate_transformation(self): choose_A = True elif errB < errA and errB < tolerance: choose_A = False + elif len(self._pairA_trans_errs) % 10 == 0 or len(self._pairB_trans_errs) % 10 == 0: + print("Computing pairing of Vive trackers with wrists") + omni.log.info( + f"Pairing Vive trackers with wrists: error of pairing A: {errA}, error of pairing B: {errB}" + ) if choose_A is None: - carb.log_info(f"error A: {errA}, error B: {errB}") return if choose_A: @@ -227,14 +231,21 @@ def _initialize_coordinate_transformation(self): if len(chosen_list) >= min_frames: cluster = select_mode_cluster(chosen_list) - carb.log_info(f"Wrist calibration: formed size {len(cluster)} cluster from {len(chosen_list)} samples") + if len(chosen_list) % 10 == 0: + print( + f"Computing wrist calibration: formed size {len(cluster)} cluster from" + f" {len(chosen_list)} samples" + ) if len(cluster) >= min_frames // 2: averaged = average_transforms(cluster) self.scene_T_lighthouse_static = averaged - carb.log_info(f"Resolved mapping: {self._vive_left_id}->Left, {self._vive_right_id}->Right") + print( + f"Wrist calibration computed. Resolved mapping: {self._vive_left_id}->Left," + f" {self._vive_right_id}->Right" + ) except Exception as e: - carb.log_error(f"Failed to initialize coordinate transformation: {e}") + omni.log.error(f"Failed to initialize coordinate transformation: {e}") def _transform_vive_data(self, device_data: dict) -> dict: """Transform Vive tracker poses to scene coordinates. @@ -304,7 +315,7 @@ def _get_palm(self, transformed_data: dict, hand: str) -> dict: Pose dictionary with 'position' and 'orientation'. """ if f"{hand}_6" not in transformed_data or f"{hand}_7" not in transformed_data: - carb.log_error(f"Joint data not found for {hand}") + # Joint data not arrived yet return self.default_pose metacarpal = transformed_data[f"{hand}_6"] proximal = transformed_data[f"{hand}_7"] @@ -422,7 +433,7 @@ def get_openxr_wrist_matrix(hand: str) -> Gf.Matrix4d: return None return joint.pose_matrix except Exception as e: - carb.log_warn(f"OpenXR {hand} wrist fetch failed: {e}") + omni.log.warn(f"OpenXR {hand} wrist fetch failed: {e}") return None diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py index b3a7401b522..f2972ec6580 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py @@ -5,6 +5,12 @@ """Retargeters for mapping input device data to robot commands.""" from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter, GR1T2RetargeterCfg +from .humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeter, G1LowerBodyStandingRetargeterCfg +from .humanoid.unitree.inspire.g1_upper_body_retargeter import UnitreeG1Retargeter, UnitreeG1RetargeterCfg +from .humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeter, + G1TriHandUpperBodyRetargeterCfg, +) from .manipulator.gripper_retargeter import GripperRetargeter, GripperRetargeterCfg from .manipulator.se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg from .manipulator.se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml index 9eb19cc11d8..6a98e472190 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml @@ -11,7 +11,7 @@ retargeting: - GR1T2_fourier_hand_6dof_L_ring_intermediate_link - GR1T2_fourier_hand_6dof_L_pinky_intermediate_link low_pass_alpha: 0.2 - scaling_factor: 1.0 + scaling_factor: 1.2 target_joint_names: - L_index_proximal_joint - L_middle_proximal_joint diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml index 29339d48861..183df868e8d 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_right_dexpilot.yml @@ -11,7 +11,7 @@ retargeting: - GR1T2_fourier_hand_6dof_R_ring_intermediate_link - GR1T2_fourier_hand_6dof_R_pinky_intermediate_link low_pass_alpha: 0.2 - scaling_factor: 1.0 + scaling_factor: 1.2 target_joint_names: - R_index_proximal_joint - R_middle_proximal_joint diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py new file mode 100644 index 00000000000..9cf6ba09c42 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py @@ -0,0 +1,28 @@ +# Copyright (c) 2022-2025, 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 torch +from dataclasses import dataclass + +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +@dataclass +class G1LowerBodyStandingRetargeterCfg(RetargeterCfg): + """Configuration for the G1 lower body standing retargeter.""" + + hip_height: float = 0.72 + """Height of the G1 robot hip in meters. The value is a fixed height suitable for G1 to do tabletop manipulation.""" + + +class G1LowerBodyStandingRetargeter(RetargeterBase): + """Provides lower body standing commands for the G1 robot.""" + + def __init__(self, cfg: G1LowerBodyStandingRetargeterCfg): + """Initialize the retargeter.""" + self.cfg = cfg + + def retarget(self, data: dict) -> torch.Tensor: + return torch.tensor([0.0, 0.0, 0.0, self.cfg.hip_height], device=self.cfg.sim_device) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_left_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_left_dexpilot.yml new file mode 100644 index 00000000000..476e20b1bc7 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_left_dexpilot.yml @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - L_thumb_tip + - L_index_tip + - L_middle_tip + - L_ring_tip + - L_pinky_tip + low_pass_alpha: 0.2 + scaling_factor: 1.2 + target_joint_names: + - L_thumb_proximal_yaw_joint + - L_thumb_proximal_pitch_joint + - L_index_proximal_joint + - L_middle_proximal_joint + - L_ring_proximal_joint + - L_pinky_proximal_joint + type: DexPilot + urdf_path: /tmp/retarget_inspire_white_left_hand.urdf + wrist_link_name: L_hand_base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_right_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_right_dexpilot.yml new file mode 100644 index 00000000000..c71cf4ed338 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/data/configs/dex-retargeting/unitree_hand_right_dexpilot.yml @@ -0,0 +1,24 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - R_thumb_tip + - R_index_tip + - R_middle_tip + - R_ring_tip + - R_pinky_tip + low_pass_alpha: 0.2 + scaling_factor: 1.2 + target_joint_names: + - R_thumb_proximal_yaw_joint + - R_thumb_proximal_pitch_joint + - R_index_proximal_joint + - R_middle_proximal_joint + - R_ring_proximal_joint + - R_pinky_proximal_joint + type: DexPilot + urdf_path: /tmp/retarget_inspire_white_right_hand.urdf + wrist_link_name: R_hand_base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_dex_retargeting_utils.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_dex_retargeting_utils.py new file mode 100644 index 00000000000..802e73aca4a --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_dex_retargeting_utils.py @@ -0,0 +1,259 @@ +# Copyright (c) 2022-2025, 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 numpy as np +import os +import torch +import yaml +from scipy.spatial.transform import Rotation as R + +import omni.log +from dex_retargeting.retargeting_config import RetargetingConfig + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# The index to map the OpenXR hand joints to the hand joints used +# in Dex-retargeting. +_HAND_JOINTS_INDEX = [1, 2, 3, 4, 5, 7, 8, 9, 10, 12, 13, 14, 15, 17, 18, 19, 20, 22, 23, 24, 25] + +# The transformation matrices to convert hand pose to canonical view. +_OPERATOR2MANO_RIGHT = np.array([ + [0, -1, 0], + [-1, 0, 0], + [0, 0, -1], +]) + +_OPERATOR2MANO_LEFT = np.array([ + [0, -1, 0], + [-1, 0, 0], + [0, 0, -1], +]) + +_LEFT_HAND_JOINT_NAMES = [ + "L_thumb_proximal_yaw_joint", + "L_thumb_proximal_pitch_joint", + "L_thumb_intermediate_joint", + "L_thumb_distal_joint", + "L_index_proximal_joint", + "L_index_intermediate_joint", + "L_middle_proximal_joint", + "L_middle_intermediate_joint", + "L_ring_proximal_joint", + "L_ring_intermediate_joint", + "L_pinky_proximal_joint", + "L_pinky_intermediate_joint", +] + + +_RIGHT_HAND_JOINT_NAMES = [ + "R_thumb_proximal_yaw_joint", + "R_thumb_proximal_pitch_joint", + "R_thumb_intermediate_joint", + "R_thumb_distal_joint", + "R_index_proximal_joint", + "R_index_intermediate_joint", + "R_middle_proximal_joint", + "R_middle_intermediate_joint", + "R_ring_proximal_joint", + "R_ring_intermediate_joint", + "R_pinky_proximal_joint", + "R_pinky_intermediate_joint", +] + + +class UnitreeG1DexRetargeting: + """A class for hand retargeting with GR1Fourier. + + Handles retargeting of OpenXRhand tracking data to GR1T2 robot hand joint angles. + """ + + def __init__( + self, + hand_joint_names: list[str], + right_hand_config_filename: str = "unitree_hand_right_dexpilot.yml", + left_hand_config_filename: str = "unitree_hand_left_dexpilot.yml", + left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/G1_inspire_assets/retarget_inspire_white_left_hand.urdf", + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/G1_inspire_assets/retarget_inspire_white_right_hand.urdf", + ): + """Initialize the hand retargeting. + + Args: + hand_joint_names: Names of hand joints in the robot model + right_hand_config_filename: Config file for right hand retargeting + left_hand_config_filename: Config file for left hand retargeting + """ + data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "data/")) + config_dir = os.path.join(data_dir, "configs/dex-retargeting") + + # Download urdf files from aws + local_left_urdf_path = retrieve_file_path(left_hand_urdf_path, force_download=True) + local_right_urdf_path = retrieve_file_path(right_hand_urdf_path, force_download=True) + + left_config_path = os.path.join(config_dir, left_hand_config_filename) + right_config_path = os.path.join(config_dir, right_hand_config_filename) + + # Update the YAML files with the correct URDF paths + self._update_yaml_with_urdf_path(left_config_path, local_left_urdf_path) + self._update_yaml_with_urdf_path(right_config_path, local_right_urdf_path) + + self._dex_left_hand = RetargetingConfig.load_from_file(left_config_path).build() + self._dex_right_hand = RetargetingConfig.load_from_file(right_config_path).build() + + self.left_dof_names = self._dex_left_hand.optimizer.robot.dof_joint_names + self.right_dof_names = self._dex_right_hand.optimizer.robot.dof_joint_names + + self.dof_names = self.left_dof_names + self.right_dof_names + self.isaac_lab_hand_joint_names = hand_joint_names + + omni.log.info("[UnitreeG1DexRetargeter] init done.") + + def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): + """Update YAML file with the correct URDF path. + + Args: + yaml_path: Path to the YAML configuration file + urdf_path: Path to the URDF file to use + """ + try: + # Read the YAML file + with open(yaml_path) as file: + config = yaml.safe_load(file) + + # Update the URDF path in the configuration + if "retargeting" in config: + config["retargeting"]["urdf_path"] = urdf_path + omni.log.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + omni.log.warn(f"Unable to find 'retargeting' section in {yaml_path}") + + # Write the updated configuration back to the file + with open(yaml_path, "w") as file: + yaml.dump(config, file) + + except Exception as e: + omni.log.error(f"Error updating YAML file {yaml_path}: {e}") + + def convert_hand_joints(self, hand_poses: dict[str, np.ndarray], operator2mano: np.ndarray) -> np.ndarray: + """Prepares the hand joints data for retargeting. + + Args: + hand_poses: Dictionary containing hand pose data with joint positions and rotations + operator2mano: Transformation matrix to convert from operator to MANO frame + + Returns: + Joint positions with shape (21, 3) + """ + joint_position = np.zeros((21, 3)) + hand_joints = list(hand_poses.values()) + for i in range(len(_HAND_JOINTS_INDEX)): + joint = hand_joints[_HAND_JOINTS_INDEX[i]] + joint_position[i] = joint[:3] + + # Convert hand pose to the canonical frame. + joint_position = joint_position - joint_position[0:1, :] + xr_wrist_quat = hand_poses.get("wrist")[3:] + # OpenXR hand uses w,x,y,z order for quaternions but scipy uses x,y,z,w order + wrist_rot = R.from_quat([xr_wrist_quat[1], xr_wrist_quat[2], xr_wrist_quat[3], xr_wrist_quat[0]]).as_matrix() + + return joint_position @ wrist_rot @ operator2mano + + def compute_ref_value(self, joint_position: np.ndarray, indices: np.ndarray, retargeting_type: str) -> np.ndarray: + """Computes reference value for retargeting. + + Args: + joint_position: Joint positions array + indices: Target link indices + retargeting_type: Type of retargeting ("POSITION" or other) + + Returns: + Reference value in cartesian space + """ + if retargeting_type == "POSITION": + return joint_position[indices, :] + else: + origin_indices = indices[0, :] + task_indices = indices[1, :] + ref_value = joint_position[task_indices, :] - joint_position[origin_indices, :] + return ref_value + + def compute_one_hand( + self, hand_joints: dict[str, np.ndarray], retargeting: RetargetingConfig, operator2mano: np.ndarray + ) -> np.ndarray: + """Computes retargeted joint angles for one hand. + + Args: + hand_joints: Dictionary containing hand joint data + retargeting: Retargeting configuration object + operator2mano: Transformation matrix from operator to MANO frame + + Returns: + Retargeted joint angles + """ + joint_pos = self.convert_hand_joints(hand_joints, operator2mano) + ref_value = self.compute_ref_value( + joint_pos, + indices=retargeting.optimizer.target_link_human_indices, + retargeting_type=retargeting.optimizer.retargeting_type, + ) + + # Enable gradient calculation and inference mode in case some other script has disabled it + # This is necessary for the retargeting to work since it uses gradient features that + # are not available in inference mode + with torch.enable_grad(): + with torch.inference_mode(False): + return retargeting.retarget(ref_value) + + def get_joint_names(self) -> list[str]: + """Returns list of all joint names.""" + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + """Returns list of left hand joint names.""" + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + """Returns list of right hand joint names.""" + return self.right_dof_names + + def get_hand_indices(self, robot) -> np.ndarray: + """Gets indices of hand joints in robot's DOF array. + + Args: + robot: Robot object containing DOF information + + Returns: + Array of joint indices + """ + return np.array([robot.dof_names.index(name) for name in self.dof_names], dtype=np.int64) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for left hand. + + Args: + left_hand_poses: Dictionary of left hand joint poses + + Returns: + Retargeted joint angles for left hand + """ + if left_hand_poses is not None: + left_hand_q = self.compute_one_hand(left_hand_poses, self._dex_left_hand, _OPERATOR2MANO_LEFT) + else: + left_hand_q = np.zeros(len(_LEFT_HAND_JOINT_NAMES)) + return left_hand_q + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for right hand. + + Args: + right_hand_poses: Dictionary of right hand joint poses + + Returns: + Retargeted joint angles for right hand + """ + if right_hand_poses is not None: + right_hand_q = self.compute_one_hand(right_hand_poses, self._dex_right_hand, _OPERATOR2MANO_RIGHT) + else: + right_hand_q = np.zeros(len(_RIGHT_HAND_JOINT_NAMES)) + return right_hand_q diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py new file mode 100644 index 00000000000..98855cc352e --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py @@ -0,0 +1,154 @@ +# Copyright (c) 2022-2025, 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 contextlib +import numpy as np +import torch +from dataclasses import dataclass + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +# This import exception is suppressed because g1_dex_retargeting_utils depends on pinocchio which is not available on windows +with contextlib.suppress(Exception): + from .g1_dex_retargeting_utils import UnitreeG1DexRetargeting + + +@dataclass +class UnitreeG1RetargeterCfg(RetargeterCfg): + """Configuration for the UnitreeG1 retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + + +class UnitreeG1Retargeter(RetargeterBase): + """Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands. + + This retargeter maps hand tracking data from OpenXR to joint commands for the GR1T2 robot's hands. + It handles both left and right hands, converting poses of the hands in OpenXR format joint angles for the GR1T2 robot's hands. + """ + + def __init__( + self, + cfg: UnitreeG1RetargeterCfg, + ): + """Initialize the UnitreeG1 hand retargeter. + + Args: + enable_visualization: If True, visualize tracked hand joints + num_open_xr_hand_joints: Number of joints tracked by OpenXR + device: PyTorch device for computations + hand_joint_names: List of robot hand joint names + """ + + self._hand_joint_names = cfg.hand_joint_names + self._hands_controller = UnitreeG1DexRetargeting(self._hand_joint_names) + + # Initialize visualization if enabled + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + self._sim_device = cfg.sim_device + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + """Convert hand joint poses to robot end-effector commands. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + + Returns: + tuple containing: + Left wrist pose + Right wrist pose in USD frame + Retargeted hand joint angles + """ + + # Access the left and right hand data using the enum key + left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] + right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) + + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + # Create array of zeros with length matching number of joint names + left_hands_pos = self._hands_controller.compute_left(left_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + left_retargeted_hand_joints[indexes] = left_hands_pos + left_hand_joints = left_retargeted_hand_joints + + right_hands_pos = self._hands_controller.compute_right(right_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + right_retargeted_hand_joints[indexes] = right_hands_pos + right_hand_joints = right_retargeted_hand_joints + retargeted_hand_joints = left_hand_joints + right_hand_joints + + # Convert numpy arrays to tensors and concatenate them + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(retargeted_hand_joints, dtype=torch.float32, device=self._sim_device) + + # Combine all tensors into a single tensor + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + wrist: Wrist pose data from OpenXR. + is_left: True for the left hand, False for the right hand. + + Returns: + Retargeted wrist pose in USD control frame. + """ + # Note: This was determined through trial, use the target quat and cloudXR quat, + # to estimate a most reasonable transformation matrix + + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + if is_left: + # Corresponds to a rotation of (0, 180, 0) in euler angles (x,y,z) + combined_quat = torch.tensor([0.7071, 0, 0.7071, 0], dtype=torch.float32) + else: + # Corresponds to a rotation of (180, 0, 0) in euler angles (x,y,z) + combined_quat = torch.tensor([0, 0.7071, 0, -0.7071], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml new file mode 100644 index 00000000000..282b5d8438b --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - thumb_tip + - index_tip + - middle_tip + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - left_hand_thumb_0_joint + - left_hand_thumb_1_joint + - left_hand_thumb_2_joint + - left_hand_middle_0_joint + - left_hand_middle_1_joint + - left_hand_index_0_joint + - left_hand_index_1_joint + type: DexPilot + urdf_path: /tmp/G1_left_hand.urdf + wrist_link_name: base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml new file mode 100644 index 00000000000..2629f9354fa --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - thumb_tip + - index_tip + - middle_tip + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - right_hand_thumb_0_joint + - right_hand_thumb_1_joint + - right_hand_thumb_2_joint + - right_hand_middle_0_joint + - right_hand_middle_1_joint + - right_hand_index_0_joint + - right_hand_index_1_joint + type: DexPilot + urdf_path: /tmp/G1_right_hand.urdf + wrist_link_name: base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py new file mode 100644 index 00000000000..78d8ed667f9 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py @@ -0,0 +1,252 @@ +# Copyright (c) 2022-2025, 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 logging +import numpy as np +import os +import torch +import yaml +from scipy.spatial.transform import Rotation as R + +import omni.log +from dex_retargeting.retargeting_config import RetargetingConfig + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# yourdfpy loads visual/collision meshes with the hand URDFs; these aren't needed for +# retargeting and clutter the logs, so we suppress them. +logging.getLogger("dex_retargeting.yourdfpy").setLevel(logging.ERROR) + +# The index to map the OpenXR hand joints to the hand joints used +# in Dex-retargeting. +_HAND_JOINTS_INDEX = [1, 2, 3, 4, 5, 7, 8, 9, 10, 12, 13, 14, 15, 17, 18, 19, 20, 22, 23, 24, 25] + +# The transformation matrices to convert hand pose to canonical view. +_OPERATOR2MANO_RIGHT = np.array([ + [0, 0, 1], + [1, 0, 0], + [0, 1, 0], +]) + +_OPERATOR2MANO_LEFT = np.array([ + [0, 0, 1], + [1, 0, 0], + [0, 1, 0], +]) + +# G1 robot hand joint names - 2 fingers and 1 thumb configuration +_LEFT_HAND_JOINT_NAMES = [ + "left_hand_thumb_0_joint", # Thumb base (yaw axis) + "left_hand_thumb_1_joint", # Thumb middle (pitch axis) + "left_hand_thumb_2_joint", # Thumb tip + "left_hand_index_0_joint", # Index finger proximal + "left_hand_index_1_joint", # Index finger distal + "left_hand_middle_0_joint", # Middle finger proximal + "left_hand_middle_1_joint", # Middle finger distal +] + +_RIGHT_HAND_JOINT_NAMES = [ + "right_hand_thumb_0_joint", # Thumb base (yaw axis) + "right_hand_thumb_1_joint", # Thumb middle (pitch axis) + "right_hand_thumb_2_joint", # Thumb tip + "right_hand_index_0_joint", # Index finger proximal + "right_hand_index_1_joint", # Index finger distal + "right_hand_middle_0_joint", # Middle finger proximal + "right_hand_middle_1_joint", # Middle finger distal +] + + +class G1TriHandDexRetargeting: + """A class for hand retargeting with G1. + + Handles retargeting of OpenXRhand tracking data to G1 robot hand joint angles. + """ + + def __init__( + self, + hand_joint_names: list[str], + right_hand_config_filename: str = "g1_hand_right_dexpilot.yml", + left_hand_config_filename: str = "g1_hand_left_dexpilot.yml", + left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_dexpilot_asset/G1_left_hand.urdf", + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_dexpilot_asset/G1_right_hand.urdf", + ): + """Initialize the hand retargeting. + + Args: + hand_joint_names: Names of hand joints in the robot model + right_hand_config_filename: Config file for right hand retargeting + left_hand_config_filename: Config file for left hand retargeting + """ + data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "data/")) + config_dir = os.path.join(data_dir, "configs/dex-retargeting") + + # Download urdf files from aws + local_left_urdf_path = retrieve_file_path(left_hand_urdf_path, force_download=True) + local_right_urdf_path = retrieve_file_path(right_hand_urdf_path, force_download=True) + + left_config_path = os.path.join(config_dir, left_hand_config_filename) + right_config_path = os.path.join(config_dir, right_hand_config_filename) + + # Update the YAML files with the correct URDF paths + self._update_yaml_with_urdf_path(left_config_path, local_left_urdf_path) + self._update_yaml_with_urdf_path(right_config_path, local_right_urdf_path) + + self._dex_left_hand = RetargetingConfig.load_from_file(left_config_path).build() + self._dex_right_hand = RetargetingConfig.load_from_file(right_config_path).build() + + self.left_dof_names = self._dex_left_hand.optimizer.robot.dof_joint_names + self.right_dof_names = self._dex_right_hand.optimizer.robot.dof_joint_names + self.dof_names = self.left_dof_names + self.right_dof_names + self.isaac_lab_hand_joint_names = hand_joint_names + + omni.log.info("[G1DexRetargeter] init done.") + + def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): + """Update YAML file with the correct URDF path. + + Args: + yaml_path: Path to the YAML configuration file + urdf_path: Path to the URDF file to use + """ + try: + # Read the YAML file + with open(yaml_path) as file: + config = yaml.safe_load(file) + + # Update the URDF path in the configuration + if "retargeting" in config: + config["retargeting"]["urdf_path"] = urdf_path + omni.log.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + omni.log.warn(f"Unable to find 'retargeting' section in {yaml_path}") + + # Write the updated configuration back to the file + with open(yaml_path, "w") as file: + yaml.dump(config, file) + + except Exception as e: + omni.log.error(f"Error updating YAML file {yaml_path}: {e}") + + def convert_hand_joints(self, hand_poses: dict[str, np.ndarray], operator2mano: np.ndarray) -> np.ndarray: + """Prepares the hand joints data for retargeting. + + Args: + hand_poses: Dictionary containing hand pose data with joint positions and rotations + operator2mano: Transformation matrix to convert from operator to MANO frame + + Returns: + Joint positions with shape (21, 3) + """ + joint_position = np.zeros((21, 3)) + hand_joints = list(hand_poses.values()) + for i, joint_index in enumerate(_HAND_JOINTS_INDEX): + joint = hand_joints[joint_index] + joint_position[i] = joint[:3] + + # Convert hand pose to the canonical frame. + joint_position = joint_position - joint_position[0:1, :] + xr_wrist_quat = hand_poses.get("wrist")[3:] + # OpenXR hand uses w,x,y,z order for quaternions but scipy uses x,y,z,w order + wrist_rot = R.from_quat([xr_wrist_quat[1], xr_wrist_quat[2], xr_wrist_quat[3], xr_wrist_quat[0]]).as_matrix() + + return joint_position @ wrist_rot @ operator2mano + + def compute_ref_value(self, joint_position: np.ndarray, indices: np.ndarray, retargeting_type: str) -> np.ndarray: + """Computes reference value for retargeting. + + Args: + joint_position: Joint positions array + indices: Target link indices + retargeting_type: Type of retargeting ("POSITION" or other) + + Returns: + Reference value in cartesian space + """ + if retargeting_type == "POSITION": + return joint_position[indices, :] + else: + origin_indices = indices[0, :] + task_indices = indices[1, :] + ref_value = joint_position[task_indices, :] - joint_position[origin_indices, :] + return ref_value + + def compute_one_hand( + self, hand_joints: dict[str, np.ndarray], retargeting: RetargetingConfig, operator2mano: np.ndarray + ) -> np.ndarray: + """Computes retargeted joint angles for one hand. + + Args: + hand_joints: Dictionary containing hand joint data + retargeting: Retargeting configuration object + operator2mano: Transformation matrix from operator to MANO frame + + Returns: + Retargeted joint angles + """ + joint_pos = self.convert_hand_joints(hand_joints, operator2mano) + ref_value = self.compute_ref_value( + joint_pos, + indices=retargeting.optimizer.target_link_human_indices, + retargeting_type=retargeting.optimizer.retargeting_type, + ) + # Enable gradient calculation and inference mode in case some other script has disabled it + # This is necessary for the retargeting to work since it uses gradient features that + # are not available in inference mode + with torch.enable_grad(): + with torch.inference_mode(False): + return retargeting.retarget(ref_value) + + def get_joint_names(self) -> list[str]: + """Returns list of all joint names.""" + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + """Returns list of left hand joint names.""" + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + """Returns list of right hand joint names.""" + return self.right_dof_names + + def get_hand_indices(self, robot) -> np.ndarray: + """Gets indices of hand joints in robot's DOF array. + + Args: + robot: Robot object containing DOF information + + Returns: + Array of joint indices + """ + return np.array([robot.dof_names.index(name) for name in self.dof_names], dtype=np.int64) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for left hand. + + Args: + left_hand_poses: Dictionary of left hand joint poses + + Returns: + Retargeted joint angles for left hand + """ + if left_hand_poses is not None: + left_hand_q = self.compute_one_hand(left_hand_poses, self._dex_left_hand, _OPERATOR2MANO_LEFT) + else: + left_hand_q = np.zeros(len(_LEFT_HAND_JOINT_NAMES)) + return left_hand_q + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for right hand. + + Args: + right_hand_poses: Dictionary of right hand joint poses + + Returns: + Retargeted joint angles for right hand + """ + if right_hand_poses is not None: + right_hand_q = self.compute_one_hand(right_hand_poses, self._dex_right_hand, _OPERATOR2MANO_RIGHT) + else: + right_hand_q = np.zeros(len(_RIGHT_HAND_JOINT_NAMES)) + return right_hand_q diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py new file mode 100644 index 00000000000..41f7f49fd9f --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py @@ -0,0 +1,166 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import contextlib +import numpy as np +import torch +from dataclasses import dataclass + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +# This import exception is suppressed because g1_dex_retargeting_utils depends on pinocchio which is not available on windows +with contextlib.suppress(Exception): + from .g1_dex_retargeting_utils import G1TriHandDexRetargeting + + +@dataclass +class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): + """Configuration for the G1UpperBody retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + + +class G1TriHandUpperBodyRetargeter(RetargeterBase): + """Retargets OpenXR data to G1 upper body commands. + + This retargeter maps hand tracking data from OpenXR to wrist and hand joint commands for the G1 robot. + It handles both left and right hands, converting poses of the hands in OpenXR format to appropriate wrist poses + and joint angles for the G1 robot's upper body. + """ + + def __init__( + self, + cfg: G1TriHandUpperBodyRetargeterCfg, + ): + """Initialize the G1 upper body retargeter. + + Args: + cfg: Configuration for the retargeter. + """ + + # Store device name for runtime retrieval + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + + # Initialize the hands controller + if cfg.hand_joint_names is not None: + self._hands_controller = G1TriHandDexRetargeting(cfg.hand_joint_names) + else: + raise ValueError("hand_joint_names must be provided in configuration") + + # Initialize visualization if enabled + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_hand_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + """Convert hand joint poses to robot end-effector commands. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + + Returns: + A tensor containing the retargeted commands: + - Left wrist pose (7) + - Right wrist pose (7) + - Hand joint angles (len(hand_joint_names)) + """ + + # Access the left and right hand data using the enum key + left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] + right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + # Handle case where wrist data is not available + if left_wrist is None or right_wrist is None: + # Set to default pose if no data available. + # pos=(0,0,0), quat=(1,0,0,0) (w,x,y,z) + default_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + if left_wrist is None: + left_wrist = default_pose + if right_wrist is None: + right_wrist = default_pose + + # Visualization if enabled + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + # Compute retargeted hand joints + left_hands_pos = self._hands_controller.compute_left(left_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + left_retargeted_hand_joints[indexes] = left_hands_pos + left_hand_joints = left_retargeted_hand_joints + + right_hands_pos = self._hands_controller.compute_right(right_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + right_retargeted_hand_joints[indexes] = right_hands_pos + right_hand_joints = right_retargeted_hand_joints + retargeted_hand_joints = left_hand_joints + right_hand_joints + + # Convert numpy arrays to tensors and store in command buffer + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(retargeted_hand_joints, dtype=torch.float32, device=self._sim_device) + + # Combine all tensors into a single tensor + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + wrist: Wrist pose data from OpenXR. + is_left: True for the left hand, False for the right hand. + + Returns: + Retargeted wrist pose in USD control frame. + """ + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + if is_left: + # Corresponds to a rotation of (0, 90, 90) in euler angles (x,y,z) + combined_quat = torch.tensor([0.7071, 0, 0.7071, 0], dtype=torch.float32) + else: + # Corresponds to a rotation of (0, -90, -90) in euler angles (x,y,z) + combined_quat = torch.tensor([0, -0.7071, 0, 0.7071], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index f2a7eed32c6..a02029645b6 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -15,6 +15,10 @@ from isaaclab.devices.gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from isaaclab.devices.keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg from isaaclab.devices.openxr.retargeters import ( + G1LowerBodyStandingRetargeter, + G1LowerBodyStandingRetargeterCfg, + G1TriHandUpperBodyRetargeter, + G1TriHandUpperBodyRetargeterCfg, GR1T2Retargeter, GR1T2RetargeterCfg, GripperRetargeter, @@ -23,6 +27,8 @@ Se3AbsRetargeterCfg, Se3RelRetargeter, Se3RelRetargeterCfg, + UnitreeG1Retargeter, + UnitreeG1RetargeterCfg, ) from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg @@ -50,6 +56,9 @@ Se3RelRetargeterCfg: Se3RelRetargeter, GripperRetargeterCfg: GripperRetargeter, GR1T2RetargeterCfg: GR1T2Retargeter, + G1TriHandUpperBodyRetargeterCfg: G1TriHandUpperBodyRetargeter, + G1LowerBodyStandingRetargeterCfg: G1LowerBodyStandingRetargeter, + UnitreeG1RetargeterCfg: UnitreeG1Retargeter, } diff --git a/source/isaaclab/isaaclab/envs/__init__.py b/source/isaaclab/isaaclab/envs/__init__.py index e69aba9d25c..2d274b8adad 100644 --- a/source/isaaclab/isaaclab/envs/__init__.py +++ b/source/isaaclab/isaaclab/envs/__init__.py @@ -39,7 +39,7 @@ For more information about the workflow design patterns, see the `Task Design Workflows`_ section. -.. _`Task Design Workflows`: https://isaac-sim.github.io/IsaacLab/source/features/task_workflows.html +.. _`Task Design Workflows`: https://docs.isaacsim.omniverse.nvidia.com/latest/introduction/workflows.html """ from . import mdp, ui diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py b/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py index 210b5139730..15f57cb4c03 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env_cfg.py @@ -225,3 +225,6 @@ class DirectMARLEnvCfg: xr: XrCfg | None = None """Configuration for viewing and interacting with the environment through an XR device.""" + + log_dir: str | None = None + """Directory for logging experiment artifacts. Defaults to None, in which case no specific log directory is set.""" diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index e985c2c6531..e43c4db7a28 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -376,9 +376,6 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) if len(reset_env_ids) > 0: self._reset_idx(reset_env_ids) - # update articulation kinematics - self.scene.write_data_to_sim() - self.sim.forward() # if sensors are added to the scene, make sure we render to reflect changes in reset if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: self.sim.render() diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py b/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py index 6b26bdc7500..33297a228af 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env_cfg.py @@ -229,3 +229,6 @@ class DirectRLEnvCfg: xr: XrCfg | None = None """Configuration for viewing and interacting with the environment through an XR device.""" + + log_dir: str | None = None + """Directory for logging experiment artifacts. Defaults to None, in which case no specific log directory is set.""" diff --git a/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py b/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py index a50e5336f9b..1a6bdacb795 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env_cfg.py @@ -131,3 +131,6 @@ class ManagerBasedEnvCfg: io_descriptors_output_dir: str | None = None """The directory to export the IO descriptors to. Defaults to None.""" + + log_dir: str | None = None + """Directory for logging experiment artifacts. Defaults to None, in which case no specific log directory is set.""" diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py index 118f588c100..634bec4cae9 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -220,9 +220,6 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: self.recorder_manager.record_pre_reset(reset_env_ids) self._reset_idx(reset_env_ids) - # update articulation kinematics - self.scene.write_data_to_sim() - self.sim.forward() # if sensors are added to the scene, make sure we render to reflect changes in reset if self.sim.has_rtx_sensors() and self.cfg.rerender_on_reset: diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py index 834d23d955a..db478e7186e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -26,15 +26,15 @@ class PinkInverseKinematicsActionCfg(ActionTermCfg): pink_controlled_joint_names: list[str] = MISSING """List of joint names or regular expression patterns that specify the joints controlled by pink IK.""" - ik_urdf_fixed_joint_names: list[str] = MISSING - """List of joint names that specify the joints to be locked in URDF.""" - hand_joint_names: list[str] = MISSING """List of joint names or regular expression patterns that specify the joints controlled by hand retargeting.""" controller: PinkIKControllerCfg = MISSING """Configuration for the Pink IK controller that will be used to solve the inverse kinematics.""" + enable_gravity_compensation: bool = True + """Whether to compensate for gravity in the Pink IK controller.""" + target_eef_link_names: dict[str, str] = MISSING """Dictionary mapping task names to controlled link names for the Pink IK controller. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index f1e9fd7a819..79490c07e42 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -5,7 +5,6 @@ from __future__ import annotations -import copy import torch from collections.abc import Sequence from typing import TYPE_CHECKING @@ -15,6 +14,7 @@ import isaaclab.utils.math as math_utils from isaaclab.assets.articulation import Articulation from isaaclab.controllers.pink_ik import PinkIKController +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask from isaaclab.managers.action_manager import ActionTerm if TYPE_CHECKING: @@ -27,8 +27,8 @@ class PinkInverseKinematicsAction(ActionTerm): r"""Pink Inverse Kinematics action term. - This action term processes the action tensor and sets these setpoints in the pink IK framework - The action tensor is ordered in the order of the tasks defined in PinkIKControllerCfg + This action term processes the action tensor and sets these setpoints in the pink IK framework. + The action tensor is ordered in the order of the tasks defined in PinkIKControllerCfg. """ cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg @@ -46,53 +46,78 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma """ super().__init__(cfg, env) - # Resolve joint IDs and names based on the configuration - self._pink_controlled_joint_ids, self._pink_controlled_joint_names = self._asset.find_joints( + self._env = env + self._sim_dt = env.sim.get_physics_dt() + + # Initialize joint information + self._initialize_joint_info() + + # Initialize IK controllers + self._initialize_ik_controllers() + + # Initialize action tensors + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self._raw_actions) + + # PhysX Articulation Floating joint indices offset from IsaacLab Articulation joint indices + self._physx_floating_joint_indices_offset = 6 + + # Pre-allocate tensors for runtime use + self._initialize_helper_tensors() + + def _initialize_joint_info(self) -> None: + """Initialize joint IDs and names based on configuration.""" + # Resolve pink controlled joints + self._isaaclab_controlled_joint_ids, self._isaaclab_controlled_joint_names = self._asset.find_joints( self.cfg.pink_controlled_joint_names ) - self.cfg.controller.joint_names = self._pink_controlled_joint_names + self.cfg.controller.joint_names = self._isaaclab_controlled_joint_names + self._isaaclab_all_joint_ids = list(range(len(self._asset.data.joint_names))) + self.cfg.controller.all_joint_names = self._asset.data.joint_names + + # Resolve hand joints self._hand_joint_ids, self._hand_joint_names = self._asset.find_joints(self.cfg.hand_joint_names) - self._joint_ids = self._pink_controlled_joint_ids + self._hand_joint_ids - self._joint_names = self._pink_controlled_joint_names + self._hand_joint_names - # Initialize the Pink IK controller - assert env.num_envs > 0, "Number of environments specified are less than 1." + # Combine all joint information + self._controlled_joint_ids = self._isaaclab_controlled_joint_ids + self._hand_joint_ids + self._controlled_joint_names = self._isaaclab_controlled_joint_names + self._hand_joint_names + + def _initialize_ik_controllers(self) -> None: + """Initialize Pink IK controllers for all environments.""" + assert self._env.num_envs > 0, "Number of environments specified are less than 1." + self._ik_controllers = [] - for _ in range(env.num_envs): + for _ in range(self._env.num_envs): self._ik_controllers.append( - PinkIKController(cfg=self.cfg.controller.copy(), robot_cfg=env.scene.cfg.robot, device=self.device) + PinkIKController( + cfg=self.cfg.controller.copy(), + robot_cfg=self._env.scene.cfg.robot, + device=self.device, + controlled_joint_indices=self._isaaclab_controlled_joint_ids, + ) ) - # Create tensors to store raw and processed actions - self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) - self._processed_actions = torch.zeros_like(self.raw_actions) - - # Get the simulation time step - self._sim_dt = env.sim.get_physics_dt() - - self.total_time = 0 # Variable to accumulate the total time - self.num_runs = 0 # Counter for the number of runs - - # Save the base_link_frame pose in the world frame as a transformation matrix in - # order to transform the desired pose of the controlled_frame to be with respect to the base_link_frame - # Shape of env.scene[self.cfg.articulation_name].data.body_link_state_w is (num_instances, num_bodies, 13) - base_link_frame_in_world_origin = env.scene[self.cfg.controller.articulation_name].data.body_link_state_w[ - :, - env.scene[self.cfg.controller.articulation_name].data.body_names.index(self.cfg.controller.base_link_name), - :7, - ] + def _initialize_helper_tensors(self) -> None: + """Pre-allocate tensors and cache values for performance optimization.""" + # Cache frequently used tensor versions of joint IDs to avoid repeated creation + self._controlled_joint_ids_tensor = torch.tensor(self._controlled_joint_ids, device=self.device) - # Get robot base link frame in env origin frame - base_link_frame_in_env_origin = copy.deepcopy(base_link_frame_in_world_origin) - base_link_frame_in_env_origin[:, :3] -= self._env.scene.env_origins + # Cache base link index to avoid string lookup every time + articulation_data = self._env.scene[self.cfg.controller.articulation_name].data + self._base_link_idx = articulation_data.body_names.index(self.cfg.controller.base_link_name) - self.base_link_frame_in_env_origin = math_utils.make_pose( - base_link_frame_in_env_origin[:, :3], math_utils.matrix_from_quat(base_link_frame_in_env_origin[:, 3:7]) + # Pre-allocate working tensors + # Count only FrameTask instances in variable_input_tasks (not all tasks) + num_frame_tasks = sum( + 1 for task in self._ik_controllers[0].cfg.variable_input_tasks if isinstance(task, FrameTask) ) + self._num_frame_tasks = num_frame_tasks + self._controlled_frame_poses = torch.zeros(num_frame_tasks, self.num_envs, 4, 4, device=self.device) - # """ - # Properties. - # """ + # Pre-allocate tensor for base frame computations + self._base_link_frame_buffer = torch.zeros(self.num_envs, 4, 4, device=self.device) + + # ==================== Properties ==================== @property def hand_joint_dim(self) -> int: @@ -153,7 +178,7 @@ def IO_descriptor(self) -> GenericActionIODescriptor: self._IO_descriptor.shape = (self.action_dim,) self._IO_descriptor.dtype = str(self.raw_actions.dtype) self._IO_descriptor.action_type = "PinkInverseKinematicsAction" - self._IO_descriptor.pink_controller_joint_names = self._pink_controlled_joint_names + self._IO_descriptor.pink_controller_joint_names = self._isaaclab_controlled_joint_names self._IO_descriptor.hand_joint_names = self._hand_joint_names self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller.__dict__ return self._IO_descriptor @@ -162,75 +187,175 @@ def IO_descriptor(self) -> GenericActionIODescriptor: # Operations. # """ - def process_actions(self, actions: torch.Tensor): + def process_actions(self, actions: torch.Tensor) -> None: """Process the input actions and set targets for each task. Args: actions: The input actions tensor. """ - # Store the raw actions + # Store raw actions self._raw_actions[:] = actions - # Make a copy of actions before modifying so that raw actions are not modified - actions_clone = actions.clone() - - # Extract hand joint positions (last 22 values) - self._target_hand_joint_positions = actions_clone[:, -self.hand_joint_dim :] - - # The action tensor provides the desired pose of the controlled_frame with respect to the env origin frame - # But the pink IK controller expects the desired pose of the controlled_frame with respect to the base_link_frame - # So we need to transform the desired pose of the controlled_frame to be with respect to the base_link_frame - - # Get the controlled_frame pose wrt to the env origin frame - all_controlled_frames_in_env_origin = [] - # The contrllers for all envs are the same, hence just using the first one to get the number of variable_input_tasks - for task_index in range(len(self._ik_controllers[0].cfg.variable_input_tasks)): - controlled_frame_in_env_origin_pos = actions_clone[ - :, task_index * self.pose_dim : task_index * self.pose_dim + self.position_dim - ] - controlled_frame_in_env_origin_quat = actions_clone[ - :, task_index * self.pose_dim + self.position_dim : (task_index + 1) * self.pose_dim - ] - controlled_frame_in_env_origin = math_utils.make_pose( - controlled_frame_in_env_origin_pos, math_utils.matrix_from_quat(controlled_frame_in_env_origin_quat) - ) - all_controlled_frames_in_env_origin.append(controlled_frame_in_env_origin) - # Stack all the controlled_frame poses in the env origin frame. Shape is (num_tasks, num_envs , 4, 4) - all_controlled_frames_in_env_origin = torch.stack(all_controlled_frames_in_env_origin) + # Extract hand joint positions directly (no cloning needed) + self._target_hand_joint_positions = actions[:, -self.hand_joint_dim :] - # Transform the controlled_frame to be with respect to the base_link_frame using batched matrix multiplication - controlled_frame_in_base_link_frame = math_utils.pose_in_A_to_pose_in_B( - all_controlled_frames_in_env_origin, math_utils.pose_inv(self.base_link_frame_in_env_origin) + # Get base link frame transformation + self.base_link_frame_in_world_rf = self._get_base_link_frame_transform() + + # Process controlled frame poses (pass original actions, no clone needed) + controlled_frame_poses = self._extract_controlled_frame_poses(actions) + transformed_poses = self._transform_poses_to_base_link_frame(controlled_frame_poses) + + # Set targets for all tasks + self._set_task_targets(transformed_poses) + + def _get_base_link_frame_transform(self) -> torch.Tensor: + """Get the base link frame transformation matrix. + + Returns: + Base link frame transformation matrix. + """ + # Get base link frame pose in world origin using cached index + articulation_data = self._env.scene[self.cfg.controller.articulation_name].data + base_link_frame_in_world_origin = articulation_data.body_link_state_w[:, self._base_link_idx, :7] + + # Transform to environment origin frame (reuse buffer to avoid allocation) + torch.sub( + base_link_frame_in_world_origin[:, :3], + self._env.scene.env_origins, + out=self._base_link_frame_buffer[:, :3, 3], ) - controlled_frame_in_base_link_frame_pos, controlled_frame_in_base_link_frame_mat = math_utils.unmake_pose( - controlled_frame_in_base_link_frame + # Copy orientation (avoid clone) + base_link_frame_quat = base_link_frame_in_world_origin[:, 3:7] + + # Create transformation matrix + return math_utils.make_pose( + self._base_link_frame_buffer[:, :3, 3], math_utils.matrix_from_quat(base_link_frame_quat) ) - # Loop through each task and set the target + def _extract_controlled_frame_poses(self, actions: torch.Tensor) -> torch.Tensor: + """Extract controlled frame poses from action tensor. + + Args: + actions: The action tensor. + + Returns: + Stacked controlled frame poses tensor. + """ + # Use pre-allocated tensor instead of list operations + for task_index in range(self._num_frame_tasks): + # Extract position and orientation for this task + pos_start = task_index * self.pose_dim + pos_end = pos_start + self.position_dim + quat_start = pos_end + quat_end = (task_index + 1) * self.pose_dim + + position = actions[:, pos_start:pos_end] + quaternion = actions[:, quat_start:quat_end] + + # Create pose matrix directly into pre-allocated tensor + self._controlled_frame_poses[task_index] = math_utils.make_pose( + position, math_utils.matrix_from_quat(quaternion) + ) + + return self._controlled_frame_poses + + def _transform_poses_to_base_link_frame(self, poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Transform poses from world frame to base link frame. + + Args: + poses: Poses in world frame. + + Returns: + Tuple of (positions, rotation_matrices) in base link frame. + """ + # Transform poses to base link frame + base_link_inv = math_utils.pose_inv(self.base_link_frame_in_world_rf) + transformed_poses = math_utils.pose_in_A_to_pose_in_B(poses, base_link_inv) + + # Extract position and rotation + positions, rotation_matrices = math_utils.unmake_pose(transformed_poses) + + return positions, rotation_matrices + + def _set_task_targets(self, transformed_poses: tuple[torch.Tensor, torch.Tensor]) -> None: + """Set targets for all tasks across all environments. + + Args: + transformed_poses: Tuple of (positions, rotation_matrices) in base link frame. + """ + positions, rotation_matrices = transformed_poses + for env_index, ik_controller in enumerate(self._ik_controllers): - for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): - if isinstance(task, FrameTask): + for frame_task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): + if isinstance(task, LocalFrameTask): + target = task.transform_target_to_base + elif isinstance(task, FrameTask): target = task.transform_target_to_world - target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy() - target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy() - task.set_target(target) + else: + continue + + # Set position and rotation targets using frame_task_index + target.translation = positions[frame_task_index, env_index, :].cpu().numpy() + target.rotation = rotation_matrices[frame_task_index, env_index, :].cpu().numpy() - def apply_actions(self): - # start_time = time.time() # Capture the time before the step + task.set_target(target) + + # ==================== Action Application ==================== + + def apply_actions(self) -> None: """Apply the computed joint positions based on the inverse kinematics solution.""" - all_envs_joint_pos_des = [] + # Compute IK solutions for all environments + ik_joint_positions = self._compute_ik_solutions() + + # Combine IK and hand joint positions + all_joint_positions = torch.cat((ik_joint_positions, self._target_hand_joint_positions), dim=1) + self._processed_actions = all_joint_positions + + # Apply gravity compensation to arm joints + if self.cfg.enable_gravity_compensation: + self._apply_gravity_compensation() + + # Apply joint position targets + self._asset.set_joint_position_target(self._processed_actions, self._controlled_joint_ids) + + def _apply_gravity_compensation(self) -> None: + """Apply gravity compensation to arm joints if not disabled in props.""" + if not self._asset.cfg.spawn.rigid_props.disable_gravity: + # Get gravity compensation forces using cached tensor + if self._asset.is_fixed_base: + gravity = torch.zeros_like( + self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._controlled_joint_ids_tensor] + ) + else: + # If floating base, then need to skip the first 6 joints (base) + gravity = self._asset.root_physx_view.get_gravity_compensation_forces()[ + :, self._controlled_joint_ids_tensor + self._physx_floating_joint_indices_offset + ] + + # Apply gravity compensation to arm joints + self._asset.set_joint_effort_target(gravity, self._controlled_joint_ids) + + def _compute_ik_solutions(self) -> torch.Tensor: + """Compute IK solutions for all environments. + + Returns: + IK joint positions tensor for all environments. + """ + ik_solutions = [] + for env_index, ik_controller in enumerate(self._ik_controllers): - curr_joint_pos = self._asset.data.joint_pos[:, self._pink_controlled_joint_ids].cpu().numpy()[env_index] - joint_pos_des = ik_controller.compute(curr_joint_pos, self._sim_dt) - all_envs_joint_pos_des.append(joint_pos_des) - all_envs_joint_pos_des = torch.stack(all_envs_joint_pos_des) + # Get current joint positions for this environment + current_joint_pos = self._asset.data.joint_pos.cpu().numpy()[env_index] + + # Compute IK solution + joint_pos_des = ik_controller.compute(current_joint_pos, self._sim_dt) + ik_solutions.append(joint_pos_des) - # Combine IK joint positions with hand joint positions - all_envs_joint_pos_des = torch.cat((all_envs_joint_pos_des, self._target_hand_joint_positions), dim=1) - self._processed_actions = all_envs_joint_pos_des + return torch.stack(ik_solutions) - self._asset.set_joint_position_target(self._processed_actions, self._joint_ids) + # ==================== Reset ==================== def reset(self, env_ids: Sequence[int] | None = None) -> None: """Reset the action term for specified environments. diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 17c5f582d1e..923fd1597ab 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -596,14 +596,16 @@ def randomize(data: torch.Tensor, params: tuple[float, float]) -> torch.Tensor: actuator_indices = slice(None) if isinstance(actuator.joint_indices, slice): global_indices = slice(None) + elif isinstance(actuator.joint_indices, torch.Tensor): + global_indices = actuator.joint_indices.to(self.asset.device) else: - global_indices = torch.tensor(actuator.joint_indices, device=self.asset.device) + raise TypeError("Actuator joint indices must be a slice or a torch.Tensor.") elif isinstance(actuator.joint_indices, slice): # we take the joints defined in the asset config global_indices = actuator_indices = torch.tensor(self.asset_cfg.joint_ids, device=self.asset.device) else: # we take the intersection of the actuator joints and the asset config joints - actuator_joint_indices = torch.tensor(actuator.joint_indices, device=self.asset.device) + actuator_joint_indices = actuator.joint_indices asset_joint_ids = torch.tensor(self.asset_cfg.joint_ids, device=self.asset.device) # the indices of the joints in the actuator that have to be randomized actuator_indices = torch.nonzero(torch.isin(actuator_joint_indices, asset_joint_ids)).view(-1) @@ -712,8 +714,56 @@ def __call__( operation=operation, distribution=distribution, ) + + # ensure the friction coefficient is non-negative + friction_coeff = torch.clamp(friction_coeff, min=0.0) + + # Always set static friction (indexed once) + static_friction_coeff = friction_coeff[env_ids[:, None], joint_ids] + + # if isaacsim version is lower than 5.0.0 we can set only the static friction coefficient + major_version = int(env.sim.get_version()[0]) + if major_version >= 5: + # Randomize raw tensors + dynamic_friction_coeff = _randomize_prop_by_op( + self.asset.data.default_joint_dynamic_friction_coeff.clone(), + friction_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + viscous_friction_coeff = _randomize_prop_by_op( + self.asset.data.default_joint_viscous_friction_coeff.clone(), + friction_distribution_params, + env_ids, + joint_ids, + operation=operation, + distribution=distribution, + ) + + # Clamp to non-negative + dynamic_friction_coeff = torch.clamp(dynamic_friction_coeff, min=0.0) + viscous_friction_coeff = torch.clamp(viscous_friction_coeff, min=0.0) + + # Ensure dynamic ≤ static (same shape before indexing) + dynamic_friction_coeff = torch.minimum(dynamic_friction_coeff, friction_coeff) + + # Index once at the end + dynamic_friction_coeff = dynamic_friction_coeff[env_ids[:, None], joint_ids] + viscous_friction_coeff = viscous_friction_coeff[env_ids[:, None], joint_ids] + else: + # For versions < 5.0.0, we do not set these values + dynamic_friction_coeff = None + viscous_friction_coeff = None + + # Single write call for all versions self.asset.write_joint_friction_coefficient_to_sim( - friction_coeff[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids + joint_friction_coeff=static_friction_coeff, + joint_dynamic_friction_coeff=dynamic_friction_coeff, + joint_viscous_friction_coeff=viscous_friction_coeff, + joint_ids=joint_ids, + env_ids=env_ids, ) # joint armature diff --git a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py index 15fc6817418..94ecdbc2461 100644 --- a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py +++ b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py @@ -52,8 +52,8 @@ def __init__(self, env: ManagerBasedEnv | DirectRLEnv, cfg: ViewerCfg): self._env = env self._cfg = copy.deepcopy(cfg) # cast viewer eye and look-at to numpy arrays - self.default_cam_eye = np.array(self._cfg.eye) - self.default_cam_lookat = np.array(self._cfg.lookat) + self.default_cam_eye = np.array(self._cfg.eye, dtype=float) + self.default_cam_lookat = np.array(self._cfg.lookat, dtype=float) # set the camera origins if self.cfg.origin_type == "env": @@ -207,9 +207,9 @@ def update_view_location(self, eye: Sequence[float] | None = None, lookat: Seque """ # store the camera view pose for later use if eye is not None: - self.default_cam_eye = np.asarray(eye) + self.default_cam_eye = np.asarray(eye, dtype=float) if lookat is not None: - self.default_cam_lookat = np.asarray(lookat) + self.default_cam_lookat = np.asarray(lookat, dtype=float) # set the camera locations viewer_origin = self.viewer_origin.detach().cpu().numpy() cam_eye = viewer_origin + self.default_cam_eye diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index e12118a36d0..15739c33ad7 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -421,7 +421,7 @@ def extras(self) -> dict[str, XFormPrim]: These are not reset or updated by the scene. They are mainly other prims that are not necessarily handled by the interactive scene, but are useful to be accessed by the user. - .. _XFormPrim: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.core/docs/index.html#isaacsim.core.prims.XFormPrim + .. _XFormPrim: https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.core.prims/docs/index.html#isaacsim.core.prims.XFormPrim """ return self._extras diff --git a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py index 0525b67a31a..3e9982135c5 100644 --- a/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/tiled_camera.py @@ -248,7 +248,9 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): # convert data buffer to warp array if isinstance(tiled_data_buffer, np.ndarray): - tiled_data_buffer = wp.array(tiled_data_buffer, device=self.device, dtype=wp.uint8) + # Let warp infer the dtype from numpy array instead of hardcoding uint8 + # Different annotators return different dtypes: RGB(uint8), depth(float32), segmentation(uint32) + tiled_data_buffer = wp.array(tiled_data_buffer, device=self.device) else: tiled_data_buffer = tiled_data_buffer.to(device=self.device) diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index ba2a019ef64..aed50d390f8 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -58,7 +58,7 @@ class ContactSensor(SensorBase): it against the object. .. _PhysX ContactReporter: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_contact_report_a_p_i.html - .. _RigidContact: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.core/docs/index.html#isaacsim.core.prims.RigidContact + .. _RigidContact: https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.core.api/docs/index.html#isaacsim.core.api.sensors.RigidContactView """ cfg: ContactSensorCfg diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py index 1a09f8b626f..2a6a438d178 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/patterns/patterns.py @@ -109,7 +109,7 @@ def bpearl_pattern(cfg: patterns_cfg.BpearlPatternCfg, device: str) -> tuple[tor The `Robosense RS-Bpearl`_ is a short-range LiDAR that has a 360 degrees x 90 degrees super wide field of view. It is designed for near-field blind-spots detection. - .. _Robosense RS-Bpearl: https://www.roscomponents.com/en/lidar-laser-scanner/267-rs-bpearl.html + .. _Robosense RS-Bpearl: https://www.roscomponents.com/product/rs-bpearl/ Args: cfg: The configuration instance for the pattern. diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py index 45502e73351..a35167fad99 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py @@ -29,7 +29,7 @@ class MeshConverter(AssetConverterBase): instancing and physics work. The rigid body component must be added to each instance and not the referenced asset (i.e. the prototype prim itself). This is because the rigid body component defines properties that are specific to each instance and cannot be shared under the referenced asset. For - more information, please check the `documentation `_. + more information, please check the `documentation `_. Due to the above, we follow the following structure: @@ -122,14 +122,15 @@ def _convert_asset(self, cfg: MeshConverterCfg): if child_mesh_prim.GetTypeName() == "Mesh": # Apply collider properties to mesh if cfg.collision_props is not None: - # -- Collision approximation to mesh - # TODO: Move this to a new Schema: https://github.com/isaac-orbit/IsaacLab/issues/163 - mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(child_mesh_prim) - mesh_collision_api.GetApproximationAttr().Set(cfg.collision_approximation) # -- Collider properties such as offset, scale, etc. schemas.define_collision_properties( prim_path=child_mesh_prim.GetPath(), cfg=cfg.collision_props, stage=stage ) + # Add collision mesh + if cfg.mesh_collision_props is not None: + schemas.define_mesh_collision_properties( + prim_path=child_mesh_prim.GetPath(), cfg=cfg.mesh_collision_props, stage=stage + ) # Delete the old Xform and make the new Xform the default prim stage.SetDefaultPrim(xform_prim) # Apply default Xform rotation to mesh -> enable to set rotation and scale diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py index af639d941a1..97e66fd46e9 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py @@ -12,35 +12,30 @@ class MeshConverterCfg(AssetConverterBaseCfg): """The configuration class for MeshConverter.""" - mass_props: schemas_cfg.MassPropertiesCfg | None = None + mass_props: schemas_cfg.MassPropertiesCfg = None """Mass properties to apply to the USD. Defaults to None. Note: If None, then no mass properties will be added. """ - rigid_props: schemas_cfg.RigidBodyPropertiesCfg | None = None + rigid_props: schemas_cfg.RigidBodyPropertiesCfg = None """Rigid body properties to apply to the USD. Defaults to None. Note: If None, then no rigid body properties will be added. """ - collision_props: schemas_cfg.CollisionPropertiesCfg | None = None + collision_props: schemas_cfg.CollisionPropertiesCfg = None """Collision properties to apply to the USD. Defaults to None. Note: If None, then no collision properties will be added. """ - - collision_approximation: str = "convexDecomposition" - """Collision approximation method to use. Defaults to "convexDecomposition". - - Valid options are: - "convexDecomposition", "convexHull", "boundingCube", - "boundingSphere", "meshSimplification", or "none" - - "none" causes no collision mesh to be added. + mesh_collision_props: schemas_cfg.MeshCollisionPropertiesCfg = None + """Mesh approximation properties to apply to all collision meshes in the USD. + Note: + If None, then no mesh approximation properties will be added. """ translation: tuple[float, float, float] = (0.0, 0.0, 0.0) diff --git a/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py b/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py index 321fbe00b0c..aa1e52be339 100644 --- a/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mjcf_converter.py @@ -31,7 +31,7 @@ class MjcfConverter(AssetConverterBase): From Isaac Sim 4.5 onwards, the extension name changed from ``omni.importer.mjcf`` to ``isaacsim.asset.importer.mjcf``. This converter class now uses the latest extension from Isaac Sim. - .. _isaacsim.asset.importer.mjcf: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_mjcf.html + .. _isaacsim.asset.importer.mjcf: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_mjcf.html """ cfg: MjcfConverterCfg diff --git a/source/isaaclab/isaaclab/sim/converters/urdf_converter.py b/source/isaaclab/isaaclab/sim/converters/urdf_converter.py index c5bf667130e..82cf55d5405 100644 --- a/source/isaaclab/isaaclab/sim/converters/urdf_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/urdf_converter.py @@ -34,7 +34,7 @@ class UrdfConverter(AssetConverterBase): From Isaac Sim 4.5 onwards, the extension name changed from ``omni.importer.urdf`` to ``isaacsim.asset.importer.urdf``. This converter class now uses the latest extension from Isaac Sim. - .. _isaacsim.asset.importer.urdf: https://docs.isaacsim.omniverse.nvidia.com/latest/robot_setup/ext_isaacsim_asset_importer_urdf.html + .. _isaacsim.asset.importer.urdf: https://docs.isaacsim.omniverse.nvidia.com/latest/importer_exporter/ext_isaacsim_asset_importer_urdf.html """ cfg: UrdfConverterCfg diff --git a/source/isaaclab/isaaclab/sim/schemas/__init__.py b/source/isaaclab/isaaclab/sim/schemas/__init__.py index bd78191ecf5..d8d04dfc478 100644 --- a/source/isaaclab/isaaclab/sim/schemas/__init__.py +++ b/source/isaaclab/isaaclab/sim/schemas/__init__.py @@ -33,11 +33,14 @@ """ from .schemas import ( + PHYSX_MESH_COLLISION_CFGS, + USD_MESH_COLLISION_CFGS, activate_contact_sensors, define_articulation_root_properties, define_collision_properties, define_deformable_body_properties, define_mass_properties, + define_mesh_collision_properties, define_rigid_body_properties, modify_articulation_root_properties, modify_collision_properties, @@ -45,16 +48,78 @@ modify_fixed_tendon_properties, modify_joint_drive_properties, modify_mass_properties, + modify_mesh_collision_properties, modify_rigid_body_properties, modify_spatial_tendon_properties, ) from .schemas_cfg import ( ArticulationRootPropertiesCfg, + BoundingCubePropertiesCfg, + BoundingSpherePropertiesCfg, CollisionPropertiesCfg, + ConvexDecompositionPropertiesCfg, + ConvexHullPropertiesCfg, DeformableBodyPropertiesCfg, FixedTendonPropertiesCfg, JointDrivePropertiesCfg, MassPropertiesCfg, + MeshCollisionPropertiesCfg, RigidBodyPropertiesCfg, + SDFMeshPropertiesCfg, SpatialTendonPropertiesCfg, + TriangleMeshPropertiesCfg, + TriangleMeshSimplificationPropertiesCfg, ) + +__all__ = [ + # articulation root + "ArticulationRootPropertiesCfg", + "define_articulation_root_properties", + "modify_articulation_root_properties", + # rigid bodies + "RigidBodyPropertiesCfg", + "define_rigid_body_properties", + "modify_rigid_body_properties", + "activate_contact_sensors", + # colliders + "CollisionPropertiesCfg", + "define_collision_properties", + "modify_collision_properties", + # deformables + "DeformableBodyPropertiesCfg", + "define_deformable_body_properties", + "modify_deformable_body_properties", + # joints + "JointDrivePropertiesCfg", + "modify_joint_drive_properties", + # mass + "MassPropertiesCfg", + "define_mass_properties", + "modify_mass_properties", + # mesh colliders + "MeshCollisionPropertiesCfg", + "define_mesh_collision_properties", + "modify_mesh_collision_properties", + # bounding cube + "BoundingCubePropertiesCfg", + # bounding sphere + "BoundingSpherePropertiesCfg", + # convex decomposition + "ConvexDecompositionPropertiesCfg", + # convex hull + "ConvexHullPropertiesCfg", + # sdf mesh + "SDFMeshPropertiesCfg", + # triangle mesh + "TriangleMeshPropertiesCfg", + # triangle mesh simplification + "TriangleMeshSimplificationPropertiesCfg", + # tendons + "FixedTendonPropertiesCfg", + "SpatialTendonPropertiesCfg", + "modify_fixed_tendon_properties", + "modify_spatial_tendon_properties", + # Constants for configs that use PhysX vs USD API + "PHYSX_MESH_COLLISION_CFGS", + "USD_MESH_COLLISION_CFGS", +] diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index a6003376122..482b6745842 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -26,6 +26,22 @@ Articulation root properties. """ +PHYSX_MESH_COLLISION_CFGS = [ + schemas_cfg.ConvexDecompositionPropertiesCfg, + schemas_cfg.ConvexHullPropertiesCfg, + schemas_cfg.TriangleMeshPropertiesCfg, + schemas_cfg.TriangleMeshSimplificationPropertiesCfg, + schemas_cfg.SDFMeshPropertiesCfg, +] + +USD_MESH_COLLISION_CFGS = [ + schemas_cfg.BoundingCubePropertiesCfg, + schemas_cfg.BoundingSpherePropertiesCfg, + schemas_cfg.ConvexDecompositionPropertiesCfg, + schemas_cfg.ConvexHullPropertiesCfg, + schemas_cfg.TriangleMeshSimplificationPropertiesCfg, +] + def define_articulation_root_properties( prim_path: str, cfg: schemas_cfg.ArticulationRootPropertiesCfg, stage: Usd.Stage | None = None @@ -934,3 +950,121 @@ def modify_deformable_body_properties( # success return True + + +""" +Collision mesh properties. +""" + + +def extract_mesh_collision_api_and_attrs(cfg): + # We use the number of user set attributes outside of the API function + # to determine which API to use in ambiguous cases, so collect them here + custom_attrs = { + key: value + for key, value in cfg.to_dict().items() + if value is not None and key not in ["usd_func", "physx_func"] + } + + use_usd_api = False + use_phsyx_api = False + + # We have some custom attributes and allow them + if len(custom_attrs) > 0 and type(cfg) in PHYSX_MESH_COLLISION_CFGS: + use_phsyx_api = True + # We have no custom attributes + elif len(custom_attrs) == 0: + if type(cfg) in USD_MESH_COLLISION_CFGS: + # Use the USD API + use_usd_api = True + else: + # Use the PhysX API + use_phsyx_api = True + + elif len(custom_attrs > 0) and type(cfg) in USD_MESH_COLLISION_CFGS: + raise ValueError("Args are specified but the USD Mesh API doesn't support them!") + + mesh_collision_appx_type = type(cfg).__name__.partition("PropertiesCfg")[0] + + if use_usd_api: + # Add approximation to the attributes as this is how USD collision mesh API is configured + api_func = cfg.usd_func + # Approximation needs to be formatted with camelCase + custom_attrs["Approximation"] = mesh_collision_appx_type[0].lower() + mesh_collision_appx_type[1:] + elif use_phsyx_api: + api_func = cfg.physx_func + else: + raise ValueError("Either USD or PhysX API should be used for mesh collision approximation!") + + return api_func, custom_attrs + + +def define_mesh_collision_properties( + prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the mesh collision schema on the input prim and set its properties. + See :func:`modify_collision_mesh_properties` for more details on how the properties are set. + Args: + prim_path : The prim path where to apply the mesh collision schema. + cfg : The configuration for the mesh collision properties. + stage : The stage where to find the prim. Defaults to None, in which case the + current stage is used. + Raises: + ValueError: When the prim path is not valid. + """ + # obtain stage + if stage is None: + stage = get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + + api_func, _ = extract_mesh_collision_api_and_attrs(cfg=cfg) + + # Only enable if not already enabled + if not api_func(prim): + api_func.Apply(prim) + + modify_mesh_collision_properties(prim_path=prim_path, cfg=cfg, stage=stage) + + +@apply_nested +def modify_mesh_collision_properties( + prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None +): + """Set properties for the mesh collision of a prim. + These properties are based on either the `Phsyx the `UsdPhysics.MeshCollisionAPI` schema. + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + .. UsdPhysics.MeshCollisionAPI: https://openusd.org/release/api/class_usd_physics_mesh_collision_a_p_i.html + Args: + prim_path : The prim path of the rigid body. This prim should be a Mesh prim. + cfg : The configuration for the mesh collision properties. + stage : The stage where to find the prim. Defaults to None, in which case the + current stage is used. + """ + # obtain stage + if stage is None: + stage = get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + + api_func, custom_attrs = extract_mesh_collision_api_and_attrs(cfg=cfg) + + # retrieve the mesh collision API + mesh_collision_api = api_func(prim) + + # set custom attributes into mesh collision API + for attr_name, value in custom_attrs.items(): + # Only "Attribute" attr should be in format "boundingSphere", so set camel_case to be False + if attr_name == "Attribute": + camel_case = False + else: + camel_case = True + safe_set_attribute_on_usd_schema(mesh_collision_api, attr_name, value, camel_case=camel_case) + + # success + return True diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py index 3fbd11cee22..a131f739e22 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py @@ -3,8 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +from dataclasses import MISSING from typing import Literal +from pxr import PhysxSchema, UsdPhysics + from isaaclab.utils import configclass @@ -426,3 +429,199 @@ class DeformableBodyPropertiesCfg: max_depenetration_velocity: float | None = None """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" + + +@configclass +class MeshCollisionPropertiesCfg: + """Properties to apply to a mesh in regards to collision. + See :meth:`set_mesh_collision_properties` for more information. + + .. note:: + If the values are MISSING, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + usd_func: callable = MISSING + + physx_func: callable = MISSING + + +@configclass +class BoundingCubePropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + +@configclass +class BoundingSpherePropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + +@configclass +class ConvexDecompositionPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxConvexDecompositionCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_decomposition_collision_a_p_i.html + """ + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + max_convex_hulls: int | None = None + """Maximum of convex hulls created during convex decomposition. + Default value is 32. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + voxel_resolution: int | None = None + """Voxel resolution used for convex decomposition. + + Defaults to 500,000 voxels. + """ + error_percentage: float | None = None + """Convex decomposition error percentage parameter. + + Defaults to 10 percent. Units are percent. + """ + shrink_wrap: bool | None = None + """Attempts to adjust the convex hull points so that they are projected onto the surface of the original graphics + mesh. + + Defaults to False. + """ + + +@configclass +class ConvexHullPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxConvexHullCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_hull_collision_a_p_i.html + """ + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + + +@configclass +class TriangleMeshPropertiesCfg(MeshCollisionPropertiesCfg): + physx_func: callable = PhysxSchema.PhysxTriangleMeshCollisionAPI + """Triangle mesh is only supported by PhysX API. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_collision_a_p_i.html + """ + + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class TriangleMeshSimplificationPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxTriangleMeshSimplificationCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_simplification_collision_a_p_i.html + """ + + simplification_metric: float | None = None + """Mesh simplification accuracy. + + Defaults to 0.55. + """ + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class SDFMeshPropertiesCfg(MeshCollisionPropertiesCfg): + physx_func: callable = PhysxSchema.PhysxSDFMeshCollisionAPI + """SDF mesh is only supported by PhysX API. + + Original PhysX documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_s_d_f_mesh_collision_a_p_i.html + + More details and steps for optimizing SDF results can be found here: + https://nvidia-omniverse.github.io/PhysX/physx/5.2.1/docs/RigidBodyCollision.html#dynamic-triangle-meshes-with-sdfs + """ + sdf_margin: float | None = None + """Margin to increase the size of the SDF relative to the bounding box diagonal length of the mesh. + + + A sdf margin value of 0.01 means the sdf boundary will be enlarged in any direction by 1% of the mesh's bounding + box diagonal length. Representing the margin relative to the bounding box diagonal length ensures that it is scale + independent. Margins allow for precise distance queries in a region slightly outside of the mesh's bounding box. + + Default value is 0.01. + Range: [0, inf) Units: dimensionless + """ + sdf_narrow_band_thickness: float | None = None + """Size of the narrow band around the mesh surface where high resolution SDF samples are available. + + Outside of the narrow band, only low resolution samples are stored. Representing the narrow band thickness as a + fraction of the mesh's bounding box diagonal length ensures that it is scale independent. A value of 0.01 is + usually large enough. The smaller the narrow band thickness, the smaller the memory consumption of the sparse SDF. + + Default value is 0.01. + Range: [0, 1] Units: dimensionless + """ + sdf_resolution: int | None = None + """The spacing of the uniformly sampled SDF is equal to the largest AABB extent of the mesh, divided by the resolution. + + Choose the lowest possible resolution that provides acceptable performance; very high resolution results in large + memory consumption, and slower cooking and simulation performance. + + Default value is 256. + Range: (1, inf) + """ + sdf_subgrid_resolution: int | None = None + """A positive subgrid resolution enables sparsity on signed-distance-fields (SDF) while a value of 0 leads to the + usage of a dense SDF. + + A value in the range of 4 to 8 is a reasonable compromise between block size and the overhead introduced by block + addressing. The smaller a block, the more memory is spent on the address table. The bigger a block, the less + precisely the sparse SDF can adapt to the mesh's surface. In most cases sparsity reduces the memory consumption of + a SDF significantly. + + Default value is 6. + Range: [0, inf) + """ diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 205129484a3..380dba26c51 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -160,6 +160,26 @@ class PhysxCfg: gpu_max_particle_contacts: int = 2**20 """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" + solve_articulation_contact_last: bool = False + """Changes the ordering inside the articulation solver. Default is False. + + PhysX employs a strict ordering for handling constraints in an articulation. The outcome of + each constraint resolution modifies the joint and associated link speeds. However, the default + ordering may not be ideal for gripping scenarios because the solver favours the constraint + types that are resolved last. This is particularly true of stiff constraint systems that are hard + to resolve without resorting to vanishingly small simulation timesteps. + + With dynamic contact resolution being such an important part of gripping, it may make + more sense to solve dynamic contact towards the end of the solver rather than at the + beginning. This parameter modifies the default ordering to enable this change. + + For more information, please check `here `__. + + .. versionadded:: v2.3 + This parameter is only available with Isaac Sim 5.1. + + """ + @configclass class RenderCfg: @@ -272,6 +292,25 @@ class RenderCfg: This is set by the variable: ``/rtx/ambientOcclusion/enabled``. """ + dome_light_upper_lower_strategy: Literal[0, 3, 4] | None = None + """Selects how to sample the Dome Light. Default is 0. + For more information, refer to the `documentation`_. + + .. _documentation: https://docs.omniverse.nvidia.com/materials-and-rendering/latest/rtx-renderer_common.html#dome-light + + Valid values are: + + * 0: **Image-Based Lighting (IBL)** - Most accurate even for high-frequency Dome Light textures. + Can introduce sampling artifacts in real-time mode. + * 3: **Limited Image-Based Lighting** - Only sampled for reflection and refraction. Fastest, but least + accurate. Good for cases where the Dome Light contributes less than other light sources. + * 4: **Approximated Image-Based Lighting** - Fast and artifacts-free sampling in real-time mode but only + works well with a low-frequency texture (e.g., a sky with no sun disc where the sun is instead a separate + Distant Light). Requires enabling Direct Lighting denoiser. + + This is set by the variable: ``/rtx/domeLight/upperLowerStrategy``. + """ + carb_settings: dict[str, Any] | None = None """A general dictionary for users to supply all carb rendering settings with native names. diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index fd23d73c01c..83277635acf 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -26,10 +26,11 @@ import omni.physx import omni.usd from isaacsim.core.api.simulation_context import SimulationContext as _SimulationContext +from isaacsim.core.simulation_manager import SimulationManager from isaacsim.core.utils.carb import get_carb_setting, set_carb_setting from isaacsim.core.utils.viewports import set_camera_view from isaacsim.core.version import get_version -from pxr import Gf, PhysxSchema, Usd, UsdPhysics +from pxr import Gf, PhysxSchema, Sdf, Usd, UsdPhysics from isaaclab.sim.utils import create_new_stage_in_memory, use_stage @@ -259,6 +260,19 @@ def __init__(self, cfg: SimulationCfg | None = None): " simulation step size if you run into physics issues." ) + # set simulation device + # note: Although Isaac Sim sets the physics device in the init function, + # it does a render call which gets the wrong device. + SimulationManager.set_physics_sim_device(self.cfg.device) + + # obtain the parsed device + # This device should be the same as "self.cfg.device". However, for cases, where users specify the device + # as "cuda" and not "cuda:X", then it fetches the current device from SimulationManager. + # Note: Since we fix the device from the configuration and don't expect users to change it at runtime, + # we can obtain the device once from the SimulationManager.get_physics_sim_device() function. + # This reduces the overhead of calling the function. + self._physics_device = SimulationManager.get_physics_sim_device() + # create a simulation context to control the simulator if float(".".join(self._isaacsim_version[2])) < 5: # stage arg is not supported before isaac sim 5.0 @@ -283,126 +297,19 @@ def __init__(self, cfg: SimulationCfg | None = None): stage=self._initial_stage, ) - def _apply_physics_settings(self): - """Sets various carb physics settings.""" - # enable hydra scene-graph instancing - # note: this allows rendering of instanceable assets on the GUI - set_carb_setting(self.carb_settings, "/persistent/omnihydra/useSceneGraphInstancing", True) - # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking - # note: dispatcher handles how threads are launched for multi-threaded physics - set_carb_setting(self.carb_settings, "/physics/physxDispatcher", True) - # disable contact processing in omni.physx - # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. - # The physics flag gets enabled when a contact sensor is created. - if hasattr(self.cfg, "disable_contact_processing"): - omni.log.warn( - "The `disable_contact_processing` attribute is deprecated and always set to True" - " to avoid unnecessary overhead. Contact processing is automatically enabled when" - " a contact sensor is created, so manual configuration is no longer required." - ) - # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts - # are always processed. The issue is reported to the PhysX team by @mmittal. - set_carb_setting(self.carb_settings, "/physics/disableContactProcessing", True) - # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them - # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags - # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry - set_carb_setting(self.carb_settings, "/physics/collisionConeCustomGeometry", False) - set_carb_setting(self.carb_settings, "/physics/collisionCylinderCustomGeometry", False) - # hide the Simulation Settings window - set_carb_setting(self.carb_settings, "/physics/autoPopupSimulationOutputWindow", False) - - def _apply_render_settings_from_cfg(self): - """Sets rtx settings specified in the RenderCfg.""" - - # define mapping of user-friendly RenderCfg names to native carb names - rendering_setting_name_mapping = { - "enable_translucency": "/rtx/translucency/enabled", - "enable_reflections": "/rtx/reflections/enabled", - "enable_global_illumination": "/rtx/indirectDiffuse/enabled", - "enable_dlssg": "/rtx-transient/dlssg/enabled", - "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", - "dlss_mode": "/rtx/post/dlss/execMode", - "enable_direct_lighting": "/rtx/directLighting/enabled", - "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", - "enable_shadows": "/rtx/shadows/enabled", - "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", - } - - not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] - - # grab the rendering mode using the following priority: - # 1. command line argument --rendering_mode, if provided - # 2. rendering_mode from Render Config, if set - # 3. lastly, default to "balanced" mode, if neither is specified - rendering_mode = get_carb_setting(self.carb_settings, "/isaaclab/rendering/rendering_mode") - if not rendering_mode: - rendering_mode = self.cfg.render.rendering_mode - if not rendering_mode: - rendering_mode = "balanced" - - # set preset settings (same behavior as the CLI arg --rendering_mode) - if rendering_mode is not None: - # check if preset is supported - supported_rendering_modes = ["performance", "balanced", "quality"] - if rendering_mode not in supported_rendering_modes: - raise ValueError( - f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." - ) - - # grab isaac lab apps path - isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") - # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder - if float(".".join(self._isaacsim_version[2])) < 5: - isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") - - # grab preset settings - preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") - with open(preset_filename) as file: - preset_dict = toml.load(file) - preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) - - # set presets - for key, value in preset_dict.items(): - key = "/" + key.replace(".", "/") # convert to carb setting format - set_carb_setting(self.carb_settings, key, value) - - # set user-friendly named settings - for key, value in vars(self.cfg.render).items(): - if value is None or key in not_carb_settings: - # skip unset settings and non-carb settings - continue - if key not in rendering_setting_name_mapping: - raise ValueError( - f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" - " need to be updated." - ) - key = rendering_setting_name_mapping[key] - set_carb_setting(self.carb_settings, key, value) - - # set general carb settings - carb_settings = self.cfg.render.carb_settings - if carb_settings is not None: - for key, value in carb_settings.items(): - if "_" in key: - key = "/" + key.replace("_", "/") # convert from python variable style string - elif "." in key: - key = "/" + key.replace(".", "/") # convert from .kit file style string - if get_carb_setting(self.carb_settings, key) is None: - raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") - set_carb_setting(self.carb_settings, key, value) + """ + Properties - Override. + """ - # set denoiser mode - if self.cfg.render.antialiasing_mode is not None: - try: - import omni.replicator.core as rep + @property + def device(self) -> str: + """Device used by the simulation. - rep.settings.set_render_rtx_realtime(antialiasing=self.cfg.render.antialiasing_mode) - except Exception: - pass - - # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. - if get_carb_setting(self.carb_settings, "/rtx/rendermode").lower() == "raytracedlighting": - set_carb_setting(self.carb_settings, "/rtx/rendermode", "RaytracedLighting") + Note: + In Omniverse, it is possible to configure multiple GPUs for rendering, while physics engine + operates on a single GPU. This function returns the device that is used for physics simulation. + """ + return self._physics_device """ Operations - New. @@ -742,6 +649,128 @@ def clear_instance(cls): Helper Functions """ + def _apply_physics_settings(self): + """Sets various carb physics settings.""" + # enable hydra scene-graph instancing + # note: this allows rendering of instanceable assets on the GUI + set_carb_setting(self.carb_settings, "/persistent/omnihydra/useSceneGraphInstancing", True) + # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking + # note: dispatcher handles how threads are launched for multi-threaded physics + set_carb_setting(self.carb_settings, "/physics/physxDispatcher", True) + # disable contact processing in omni.physx + # note: we disable it by default to avoid the overhead of contact processing when it isn't needed. + # The physics flag gets enabled when a contact sensor is created. + if hasattr(self.cfg, "disable_contact_processing"): + omni.log.warn( + "The `disable_contact_processing` attribute is deprecated and always set to True" + " to avoid unnecessary overhead. Contact processing is automatically enabled when" + " a contact sensor is created, so manual configuration is no longer required." + ) + # FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts + # are always processed. The issue is reported to the PhysX team by @mmittal. + set_carb_setting(self.carb_settings, "/physics/disableContactProcessing", True) + # disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them + # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags + # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry + set_carb_setting(self.carb_settings, "/physics/collisionConeCustomGeometry", False) + set_carb_setting(self.carb_settings, "/physics/collisionCylinderCustomGeometry", False) + # hide the Simulation Settings window + set_carb_setting(self.carb_settings, "/physics/autoPopupSimulationOutputWindow", False) + + def _apply_render_settings_from_cfg(self): + """Sets rtx settings specified in the RenderCfg.""" + + # define mapping of user-friendly RenderCfg names to native carb names + rendering_setting_name_mapping = { + "enable_translucency": "/rtx/translucency/enabled", + "enable_reflections": "/rtx/reflections/enabled", + "enable_global_illumination": "/rtx/indirectDiffuse/enabled", + "enable_dlssg": "/rtx-transient/dlssg/enabled", + "enable_dl_denoiser": "/rtx-transient/dldenoiser/enabled", + "dlss_mode": "/rtx/post/dlss/execMode", + "enable_direct_lighting": "/rtx/directLighting/enabled", + "samples_per_pixel": "/rtx/directLighting/sampledLighting/samplesPerPixel", + "enable_shadows": "/rtx/shadows/enabled", + "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", + "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", + } + + not_carb_settings = ["rendering_mode", "carb_settings", "antialiasing_mode"] + + # grab the rendering mode using the following priority: + # 1. command line argument --rendering_mode, if provided + # 2. rendering_mode from Render Config, if set + # 3. lastly, default to "balanced" mode, if neither is specified + rendering_mode = get_carb_setting(self.carb_settings, "/isaaclab/rendering/rendering_mode") + if not rendering_mode: + rendering_mode = self.cfg.render.rendering_mode + if not rendering_mode: + rendering_mode = "balanced" + + # set preset settings (same behavior as the CLI arg --rendering_mode) + if rendering_mode is not None: + # check if preset is supported + supported_rendering_modes = ["performance", "balanced", "quality"] + if rendering_mode not in supported_rendering_modes: + raise ValueError( + f"RenderCfg rendering mode '{rendering_mode}' not in supported modes {supported_rendering_modes}." + ) + + # grab isaac lab apps path + isaaclab_app_exp_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), *[".."] * 4, "apps") + # for Isaac Sim 4.5 compatibility, we use the 4.5 rendering mode app files in a different folder + if float(".".join(self._isaacsim_version[2])) < 5: + isaaclab_app_exp_path = os.path.join(isaaclab_app_exp_path, "isaacsim_4_5") + + # grab preset settings + preset_filename = os.path.join(isaaclab_app_exp_path, f"rendering_modes/{rendering_mode}.kit") + with open(preset_filename) as file: + preset_dict = toml.load(file) + preset_dict = dict(flatdict.FlatDict(preset_dict, delimiter=".")) + + # set presets + for key, value in preset_dict.items(): + key = "/" + key.replace(".", "/") # convert to carb setting format + set_carb_setting(self.carb_settings, key, value) + + # set user-friendly named settings + for key, value in vars(self.cfg.render).items(): + if value is None or key in not_carb_settings: + # skip unset settings and non-carb settings + continue + if key not in rendering_setting_name_mapping: + raise ValueError( + f"'{key}' in RenderCfg not found. Note: internal 'rendering_setting_name_mapping' dictionary might" + " need to be updated." + ) + key = rendering_setting_name_mapping[key] + set_carb_setting(self.carb_settings, key, value) + + # set general carb settings + carb_settings = self.cfg.render.carb_settings + if carb_settings is not None: + for key, value in carb_settings.items(): + if "_" in key: + key = "/" + key.replace("_", "/") # convert from python variable style string + elif "." in key: + key = "/" + key.replace(".", "/") # convert from .kit file style string + if get_carb_setting(self.carb_settings, key) is None: + raise ValueError(f"'{key}' in RenderCfg.general_parameters does not map to a carb setting.") + set_carb_setting(self.carb_settings, key, value) + + # set denoiser mode + if self.cfg.render.antialiasing_mode is not None: + try: + import omni.replicator.core as rep + + rep.settings.set_render_rtx_realtime(antialiasing=self.cfg.render.antialiasing_mode) + except Exception: + pass + + # WAR: Ensure /rtx/renderMode RaytracedLighting is correctly cased. + if get_carb_setting(self.carb_settings, "/rtx/rendermode").lower() == "raytracedlighting": + set_carb_setting(self.carb_settings, "/rtx/rendermode", "RaytracedLighting") + def _set_additional_physx_params(self): """Sets additional PhysX parameters that are not directly supported by the parent class.""" # obtain the physics scene api @@ -758,6 +787,11 @@ def _set_additional_physx_params(self): physx_scene_api.CreateGpuCollisionStackSizeAttr(self.cfg.physx.gpu_collision_stack_size) # -- Improved determinism by PhysX physx_scene_api.CreateEnableEnhancedDeterminismAttr(self.cfg.physx.enable_enhanced_determinism) + # -- Set solve_articulation_contact_last by add attribute to the PhysxScene prim, and add attribute there. + physx_prim = physx_scene_api.GetPrim() + physx_prim.CreateAttribute("physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool).Set( + self.cfg.physx.solve_articulation_contact_last + ) # -- Gravity # note: Isaac sim only takes the "up-axis" as the gravity direction. But physics allows any direction so we @@ -783,7 +817,7 @@ def _set_additional_physx_params(self): # create the default physics material # this material is used when no material is specified for a primitive - # check: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials + # check: https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.sim.html#isaaclab.sim.SimulationCfg.physics_material material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" self.cfg.physics_material.func(material_path, self.cfg.physics_material) # bind the physics material to the scene diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py index 052a2be4f88..966efec76b8 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py @@ -49,7 +49,7 @@ .. _Material Definition Language (MDL): https://raytracing-docs.nvidia.com/mdl/introduction/index.html#mdl_introduction# .. _Materials: https://docs.omniverse.nvidia.com/materials-and-rendering/latest/materials.html -.. _physics material: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials +.. _physics material: https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.sim.html#isaaclab.sim.SimulationCfg.physics_material .. _USD Material Binding API: https://openusd.org/dev/api/class_usd_shade_material_binding_a_p_i.html .. _Physics Scene: https://openusd.org/dev/api/usd_physics_page_front.html """ diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py index 8b6e6a30b2d..81351305ab7 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py @@ -31,10 +31,6 @@ class RigidBodyMaterialCfg(PhysicsMaterialCfg): """Physics material parameters for rigid bodies. See :meth:`spawn_rigid_body_material` for more information. - - Note: - The default values are the `default values used by PhysX 5 - `__. """ func: Callable = physics_materials.spawn_rigid_body_material @@ -89,9 +85,6 @@ class DeformableBodyMaterialCfg(PhysicsMaterialCfg): See :meth:`spawn_deformable_body_material` for more information. - Note: - The default values are the `default values used by PhysX 5 - `__. """ func: Callable = physics_materials.spawn_deformable_body_material diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index 2f90030ab3d..189b687f889 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -24,10 +24,6 @@ class PinholeCameraCfg(SpawnerCfg): ..note :: Focal length as well as the aperture sizes and offsets are set as a tenth of the world unit. In our case, the world unit is Meter s.t. all of these values are set in cm. - - .. note:: - The default values are taken from the `Replicator camera `__ - function. """ func: Callable = sensors.spawn_camera @@ -170,7 +166,7 @@ class FisheyeCameraCfg(PinholeCameraCfg): `camera documentation `__. .. note:: - The default values are taken from the `Replicator camera `__ + The default values are taken from the `Replicator camera `__ function. .. _fish-eye camera: https://en.wikipedia.org/wiki/Fisheye_lens diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py index 9b75664c878..f4fa156704a 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py @@ -244,7 +244,7 @@ def _spawn_geom_from_prim_type( instancing and physics work. The rigid body component must be added to each instance and not the referenced asset (i.e. the prototype prim itself). This is because the rigid body component defines properties that are specific to each instance and cannot be shared under the referenced asset. For - more information, please check the `documentation `_. + more information, please check the `documentation `_. Due to the above, we follow the following structure: diff --git a/source/isaaclab/isaaclab/sim/utils.py b/source/isaaclab/isaaclab/sim/utils.py index debda3ec807..faf49ba3d6a 100644 --- a/source/isaaclab/isaaclab/sim/utils.py +++ b/source/isaaclab/isaaclab/sim/utils.py @@ -394,7 +394,7 @@ def bind_physics_material( The function is decorated with :meth:`apply_nested` to allow applying the function to a prim path and all its descendants. - .. _Physics material: https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html#physics-materials + .. _Physics material: https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.sim.html#isaaclab.sim.SimulationCfg.physics_material Args: prim_path: The prim path where to apply the material. diff --git a/source/isaaclab/isaaclab/utils/configclass.py b/source/isaaclab/isaaclab/utils/configclass.py index 091b9862474..bce95d961c7 100644 --- a/source/isaaclab/isaaclab/utils/configclass.py +++ b/source/isaaclab/isaaclab/utils/configclass.py @@ -259,6 +259,9 @@ def _validate(obj: object, prefix: str = "") -> list[str]: """ missing_fields = [] + if type(obj).__name__ == "MeshConverterCfg": + return missing_fields + if type(obj) is type(MISSING): missing_fields.append(prefix) return missing_fields @@ -455,10 +458,15 @@ def _skippable_class_member(key: str, value: Any, hints: dict | None = None) -> # check for class methods if isinstance(value, types.MethodType): return True + + if "CollisionAPI" in value.__name__: + return False + # check for instance methods signature = inspect.signature(value) if "self" in signature.parameters or "cls" in signature.parameters: return True + # skip property methods if isinstance(value, property): return True diff --git a/source/isaaclab/isaaclab/utils/io/__init__.py b/source/isaaclab/isaaclab/utils/io/__init__.py index 1808eb1df7b..d2e03831231 100644 --- a/source/isaaclab/isaaclab/utils/io/__init__.py +++ b/source/isaaclab/isaaclab/utils/io/__init__.py @@ -7,5 +7,5 @@ Submodules for files IO operations. """ -from .pkl import dump_pickle, load_pickle +from .torchscript import load_torchscript_model from .yaml import dump_yaml, load_yaml diff --git a/source/isaaclab/isaaclab/utils/io/torchscript.py b/source/isaaclab/isaaclab/utils/io/torchscript.py new file mode 100644 index 00000000000..df5fe454bf3 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/io/torchscript.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""TorchScript I/O utilities.""" + +import os +import torch + + +def load_torchscript_model(model_path: str, device: str = "cpu") -> torch.nn.Module: + """Load a TorchScript model from the specified path. + + This function only loads TorchScript models (.pt or .pth files created with torch.jit.save). + It will not work with raw PyTorch checkpoints (.pth files created with torch.save). + + Args: + model_path (str): Path to the TorchScript model file (.pt or .pth) + device (str, optional): Device to load the model on. Defaults to 'cpu'. + + Returns: + torch.nn.Module: The loaded TorchScript model in evaluation mode + + Raises: + FileNotFoundError: If the model file does not exist + """ + if not os.path.exists(model_path): + raise FileNotFoundError(f"TorchScript model file not found: {model_path}") + + try: + model = torch.jit.load(model_path, map_location=device) + model.eval() + print(f"Successfully loaded TorchScript model from {model_path}") + return model + except Exception as e: + print(f"Error loading TorchScript model: {e}") + return None diff --git a/source/isaaclab/isaaclab/utils/modifiers/modifier.py b/source/isaaclab/isaaclab/utils/modifiers/modifier.py index efff7b4d8c9..6121d69ed1f 100644 --- a/source/isaaclab/isaaclab/utils/modifiers/modifier.py +++ b/source/isaaclab/isaaclab/utils/modifiers/modifier.py @@ -123,11 +123,11 @@ class DigitalFilter(ModifierBase): where :math:`\alpha` is a smoothing parameter between 0 and 1. Typically, the value of :math:`\alpha` is chosen based on the desired cut-off frequency of the filter. - This filter can be implemented as a digital filter with the coefficients :math:`A = [\alpha]` and + This filter can be implemented as a digital filter with the coefficients :math:`A = [-\alpha]` and :math:`B = [1 - \alpha]`. """ - def __init__(self, cfg: modifier_cfg.DigitalFilterCfg, data_dim: tuple[int, ...], device: str) -> None: + def __init__(self, cfg: modifier_cfg.DigitalFilterCfg, data_dim: tuple[int, ...], device: str): """Initializes digital filter. Args: diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index c78f9817245..b10d3a302cb 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -35,7 +35,7 @@ "einops", # needed for transformers, doesn't always auto-install "warp-lang", # make sure this is consistent with isaac sim version - "pillow==11.2.1", + "pillow==11.3.0", # livestream "starlette==0.45.3", # testing @@ -46,13 +46,14 @@ "flaky", ] -# Append Linux x86_64–only deps via PEP 508 markers -X64 = "platform_machine in 'x86_64,AMD64'" +# Append Linux x86_64 and ARM64 deps via PEP 508 markers +SUPPORTED_ARCHS_ARM = "platform_machine in 'x86_64,AMD64,aarch64,arm64'" +SUPPORTED_ARCHS = "platform_machine in 'x86_64,AMD64'" INSTALL_REQUIRES += [ # required by isaaclab.isaaclab.controllers.pink_ik - f"pin-pink==3.1.0 ; platform_system == 'Linux' and ({X64})", + f"pin-pink==3.1.0 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", # required by isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1_t2_dex_retargeting_utils - f"dex-retargeting==0.4.6 ; platform_system == 'Linux' and ({X64})", + f"dex-retargeting==0.4.6 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS})", ] PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu128"] @@ -78,6 +79,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/source/isaaclab/test/app/test_non_headless_launch.py b/source/isaaclab/test/app/test_non_headless_launch.py new file mode 100644 index 00000000000..52c35a10916 --- /dev/null +++ b/source/isaaclab/test/app/test_non_headless_launch.py @@ -0,0 +1,65 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script checks if the app can be launched with non-headless app and start the simulation. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import pytest + +from isaaclab.app import AppLauncher + +# launch omniverse app +app_launcher = AppLauncher(experience="isaaclab.python.kit", headless=True) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils import configclass + + +@configclass +class SensorsSceneCfg(InteractiveSceneCfg): + """Design the scene with sensors on the robot.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + +def run_simulator( + sim: sim_utils.SimulationContext, +): + """Run the simulator.""" + + count = 0 + + # Simulate physics + while simulation_app.is_running() and count < 100: + # perform step + sim.step() + count += 1 + + +@pytest.mark.isaacsim_ci +def test_non_headless_launch(): + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.005) + sim = sim_utils.SimulationContext(sim_cfg) + # design scene + scene_cfg = SensorsSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + print(scene) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim) diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index 30a97b3275c..dfacff5d2ec 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -1879,7 +1879,6 @@ def test_write_joint_state_data_consistency(sim, num_articulations, device, grav rand_joint_vel = vel_dist.sample() articulation.write_joint_state_to_sim(rand_joint_pos, rand_joint_vel) - articulation.root_physx_view.get_jacobians() # make sure valued updated assert torch.count_nonzero(original_body_states[:, 1:] != articulation.data.body_state_w[:, 1:]) > ( len(original_body_states[:, 1:]) / 2 @@ -1998,10 +1997,16 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device) viscous_friction = torch.rand(num_articulations, articulation.num_joints, device=device) friction = torch.rand(num_articulations, articulation.num_joints, device=device) + + # Guarantee that the dynamic friction is not greater than the static friction + dynamic_friction = torch.min(dynamic_friction, friction) + + # The static friction must be set first to be sure the dynamic friction is not greater than static + # when both are set. + articulation.write_joint_friction_coefficient_to_sim(friction) if int(get_version()[2]) >= 5: articulation.write_joint_dynamic_friction_coefficient_to_sim(dynamic_friction) articulation.write_joint_viscous_friction_coefficient_to_sim(viscous_friction) - articulation.write_joint_friction_coefficient_to_sim(friction) articulation.write_data_to_sim() for _ in range(100): @@ -2011,9 +2016,58 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground articulation.update(sim.cfg.dt) if int(get_version()[2]) >= 5: - assert torch.allclose(articulation.data.joint_dynamic_friction_coeff, dynamic_friction) - assert torch.allclose(articulation.data.joint_viscous_friction_coeff, viscous_friction) - assert torch.allclose(articulation.data.joint_friction_coeff, friction) + friction_props_from_sim = articulation.root_physx_view.get_dof_friction_properties() + joint_friction_coeff_sim = friction_props_from_sim[:, :, 0] + joint_dynamic_friction_coeff_sim = friction_props_from_sim[:, :, 1] + joint_viscous_friction_coeff_sim = friction_props_from_sim[:, :, 2] + assert torch.allclose(joint_dynamic_friction_coeff_sim, dynamic_friction.cpu()) + assert torch.allclose(joint_viscous_friction_coeff_sim, viscous_friction.cpu()) + else: + joint_friction_coeff_sim = articulation.root_physx_view.get_dof_friction_properties() + + assert torch.allclose(joint_friction_coeff_sim, friction.cpu()) + + # For Isaac Sim >= 5.0: also test the combined API that can set dynamic and viscous via + # write_joint_friction_coefficient_to_sim; reset the sim to isolate this path. + if int(get_version()[2]) >= 5: + # Reset simulator to ensure a clean state for the alternative API path + sim.reset() + + # Warm up a few steps to populate buffers + for _ in range(100): + sim.step() + articulation.update(sim.cfg.dt) + + # New random coefficients + dynamic_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) + viscous_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) + friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device) + + # Guarantee that the dynamic friction is not greater than the static friction + dynamic_friction_2 = torch.min(dynamic_friction_2, friction_2) + + # Use the combined setter to write all three at once + articulation.write_joint_friction_coefficient_to_sim( + joint_friction_coeff=friction_2, + joint_dynamic_friction_coeff=dynamic_friction_2, + joint_viscous_friction_coeff=viscous_friction_2, + ) + articulation.write_data_to_sim() + + # Step to let sim ingest new params and refresh data buffers + for _ in range(100): + sim.step() + articulation.update(sim.cfg.dt) + + friction_props_from_sim_2 = articulation.root_physx_view.get_dof_friction_properties() + joint_friction_coeff_sim_2 = friction_props_from_sim_2[:, :, 0] + friction_dynamic_coef_sim_2 = friction_props_from_sim_2[:, :, 1] + friction_viscous_coeff_sim_2 = friction_props_from_sim_2[:, :, 2] + + # Validate values propagated + assert torch.allclose(friction_viscous_coeff_sim_2, viscous_friction_2.cpu()) + assert torch.allclose(friction_dynamic_coef_sim_2, dynamic_friction_2.cpu()) + assert torch.allclose(joint_friction_coeff_sim_2, friction_2.cpu()) if __name__ == "__main__": diff --git a/source/isaaclab/test/assets/test_rigid_object.py b/source/isaaclab/test/assets/test_rigid_object.py index 3ed69b88e32..6a0dc77b861 100644 --- a/source/isaaclab/test/assets/test_rigid_object.py +++ b/source/isaaclab/test/assets/test_rigid_object.py @@ -22,6 +22,7 @@ import isaacsim.core.utils.prims as prim_utils import pytest +from flaky import flaky import isaaclab.sim as sim_utils from isaaclab.assets import RigidObject, RigidObjectCfg @@ -857,6 +858,7 @@ def test_gravity_vec_w(num_cubes, device, gravity_enabled): @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("with_offset", [True, False]) @pytest.mark.isaacsim_ci +@flaky(max_runs=3, min_passes=1) def test_body_root_state_properties(num_cubes, device, with_offset): """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" with build_simulation_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: diff --git a/source/isaaclab/test/controllers/test_controller_utils.py b/source/isaaclab/test/controllers/test_controller_utils.py new file mode 100644 index 00000000000..9646b0e9398 --- /dev/null +++ b/source/isaaclab/test/controllers/test_controller_utils.py @@ -0,0 +1,662 @@ +# Copyright (c) 2022-2025, 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 cases for Isaac Lab controller utilities.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import os + +# Import the function to test +import tempfile +import torch + +import pytest + +from isaaclab.controllers.utils import change_revolute_to_fixed, change_revolute_to_fixed_regex +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.io.torchscript import load_torchscript_model + + +@pytest.fixture +def mock_urdf_content(): + """Create mock URDF content for testing.""" + return """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + + +@pytest.fixture +def test_urdf_file(mock_urdf_content): + """Create a temporary URDF file for testing.""" + # Create a temporary directory for test files + test_dir = tempfile.mkdtemp() + + # Create the test URDF file + test_urdf_path = os.path.join(test_dir, "test_robot.urdf") + with open(test_urdf_path, "w") as f: + f.write(mock_urdf_content) + + yield test_urdf_path + + # Clean up the temporary directory and all its contents + import shutil + + shutil.rmtree(test_dir) + + +# ============================================================================= +# Test cases for change_revolute_to_fixed function +# ============================================================================= + + +def test_single_joint_conversion(test_urdf_file, mock_urdf_content): + """Test converting a single revolute joint to fixed.""" + # Test converting shoulder_to_elbow joint + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted + assert '' in modified_content + assert '' not in modified_content + + # Check that other revolute joints remain unchanged + assert '' in modified_content + assert '' in modified_content + + +def test_multiple_joints_conversion(test_urdf_file, mock_urdf_content): + """Test converting multiple revolute joints to fixed.""" + # Test converting multiple joints + fixed_joints = ["base_to_shoulder", "elbow_to_wrist"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that both joints were converted + assert '' in modified_content + assert '' in modified_content + assert '' not in modified_content + assert '' not in modified_content + + # Check that the middle joint remains unchanged + assert '' in modified_content + + +def test_non_existent_joint(test_urdf_file, mock_urdf_content): + """Test behavior when trying to convert a non-existent joint.""" + # Try to convert a joint that doesn't exist + fixed_joints = ["non_existent_joint"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_mixed_existent_and_non_existent_joints(test_urdf_file, mock_urdf_content): + """Test converting a mix of existent and non-existent joints.""" + # Try to convert both existent and non-existent joints + fixed_joints = ["base_to_shoulder", "non_existent_joint", "elbow_to_wrist"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that existent joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-existent joint didn't cause issues + assert '' not in modified_content + + +def test_already_fixed_joint(test_urdf_file, mock_urdf_content): + """Test behavior when trying to convert an already fixed joint.""" + # Try to convert a joint that is already fixed + fixed_joints = ["wrist_to_gripper"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged (no conversion happened) + assert modified_content == mock_urdf_content + + +def test_empty_joints_list(test_urdf_file, mock_urdf_content): + """Test behavior when passing an empty list of joints.""" + # Try to convert with empty list + fixed_joints = [] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_file_not_found(test_urdf_file): + """Test behavior when URDF file doesn't exist.""" + non_existent_path = os.path.join(os.path.dirname(test_urdf_file), "non_existent.urdf") + fixed_joints = ["base_to_shoulder"] + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + change_revolute_to_fixed(non_existent_path, fixed_joints) + + +def test_preserve_other_content(test_urdf_file): + """Test that other content in the URDF file is preserved.""" + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that other content is preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_joint_attributes_preserved(test_urdf_file): + """Test that joint attributes other than type are preserved.""" + fixed_joints = ["base_to_shoulder"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted but other attributes preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + +# ============================================================================= +# Test cases for change_revolute_to_fixed_regex function +# ============================================================================= + + +def test_regex_single_joint_conversion(test_urdf_file, mock_urdf_content): + """Test converting a single revolute joint to fixed using regex pattern.""" + # Test converting shoulder_to_elbow joint using exact match + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted + assert '' in modified_content + assert '' not in modified_content + + # Check that other revolute joints remain unchanged + assert '' in modified_content + assert '' in modified_content + + +def test_regex_pattern_matching(test_urdf_file, mock_urdf_content): + """Test converting joints using regex patterns.""" + # Test converting joints that contain "to" in their name + fixed_joints = [r".*to.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that all joints with "to" in the name were converted + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_regex_multiple_patterns(test_urdf_file, mock_urdf_content): + """Test converting joints using multiple regex patterns.""" + # Test converting joints that start with "base" or end with "wrist" + fixed_joints = [r"^base.*", r".*wrist$"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that matching joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-matching joints remain unchanged + assert '' in modified_content + + +def test_regex_case_sensitive_matching(test_urdf_file, mock_urdf_content): + """Test that regex matching is case sensitive.""" + # Test with uppercase pattern that won't match lowercase joint names + fixed_joints = [r".*TO.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that no joints were converted (case sensitive) + assert modified_content == mock_urdf_content + + +def test_regex_partial_word_matching(test_urdf_file, mock_urdf_content): + """Test converting joints using partial word matching.""" + # Test converting joints that contain "shoulder" in their name + fixed_joints = [r".*shoulder.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that shoulder-related joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that other joints remain unchanged + assert '' in modified_content + + +def test_regex_no_matches(test_urdf_file, mock_urdf_content): + """Test behavior when regex patterns don't match any joints.""" + # Test with pattern that won't match any joint names + fixed_joints = [r"^nonexistent.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_regex_empty_patterns_list(test_urdf_file, mock_urdf_content): + """Test behavior when passing an empty list of regex patterns.""" + # Try to convert with empty list + fixed_joints = [] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_regex_file_not_found(test_urdf_file): + """Test behavior when URDF file doesn't exist for regex function.""" + non_existent_path = os.path.join(os.path.dirname(test_urdf_file), "non_existent.urdf") + fixed_joints = [r".*to.*"] + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + change_revolute_to_fixed_regex(non_existent_path, fixed_joints) + + +def test_regex_preserve_other_content(test_urdf_file): + """Test that other content in the URDF file is preserved with regex function.""" + fixed_joints = [r".*shoulder.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that other content is preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_regex_joint_attributes_preserved(test_urdf_file): + """Test that joint attributes other than type are preserved with regex function.""" + fixed_joints = [r"^base.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted but other attributes preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + +def test_regex_complex_pattern(test_urdf_file, mock_urdf_content): + """Test converting joints using a complex regex pattern.""" + # Test converting joints that have "to" and end with a word starting with "w" + fixed_joints = [r".*to.*w.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that matching joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-matching joints remain unchanged + assert '' in modified_content + + +def test_regex_already_fixed_joint(test_urdf_file, mock_urdf_content): + """Test behavior when regex pattern matches an already fixed joint.""" + # Try to convert joints that contain "gripper" (which is already fixed) + fixed_joints = [r".*gripper.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged (no conversion happened) + assert modified_content == mock_urdf_content + + +def test_regex_special_characters(test_urdf_file, mock_urdf_content): + """Test regex patterns with special characters.""" + # Test with pattern that includes special regex characters + fixed_joints = [r".*to.*"] # This should match joints with "to" + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that joints with "to" were converted + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +# ============================================================================= +# Test cases for load_torchscript_model function +# ============================================================================= + + +@pytest.fixture +def policy_model_path(): + """Path to the test TorchScript model.""" + _policy_path = f"{ISAACLAB_NUCLEUS_DIR}/Policies/Agile/agile_locomotion.pt" + return retrieve_file_path(_policy_path) + + +def test_load_torchscript_model_success(policy_model_path): + """Test successful loading of a TorchScript model.""" + model = load_torchscript_model(policy_model_path) + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + + +def test_load_torchscript_model_cpu_device(policy_model_path): + """Test loading TorchScript model on CPU device.""" + model = load_torchscript_model(policy_model_path, device="cpu") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + + +def test_load_torchscript_model_cuda_device(policy_model_path): + """Test loading TorchScript model on CUDA device if available.""" + if torch.cuda.is_available(): + model = load_torchscript_model(policy_model_path, device="cuda") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + else: + # Skip test if CUDA is not available + pytest.skip("CUDA not available") + + +def test_load_torchscript_model_file_not_found(): + """Test behavior when TorchScript model file doesn't exist.""" + non_existent_path = "non_existent_model.pt" + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + load_torchscript_model(non_existent_path) + + +def test_load_torchscript_model_invalid_file(): + """Test behavior when trying to load an invalid TorchScript file.""" + # Create a temporary file with invalid content + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file.write(b"invalid torchscript content") + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) + + +def test_load_torchscript_model_empty_file(): + """Test behavior when trying to load an empty TorchScript file.""" + # Create a temporary empty file + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) + + +def test_load_torchscript_model_different_device_mapping(policy_model_path): + """Test loading model with different device mapping.""" + # Test with specific device mapping + model = load_torchscript_model(policy_model_path, device="cpu") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + +def test_load_torchscript_model_evaluation_mode(policy_model_path): + """Test that loaded model is in evaluation mode.""" + model = load_torchscript_model(policy_model_path) + + # Check that model is in evaluation mode + assert model.training is False + + # Verify we can set it to training mode and back + model.train() + assert model.training is True + model.eval() + assert model.training is False + + +def test_load_torchscript_model_inference_capability(policy_model_path): + """Test that loaded model can perform inference.""" + model = load_torchscript_model(policy_model_path) + + # Check that model was loaded successfully + assert model is not None + + # Try to create a dummy input tensor (actual input shape depends on the model) + # This is a basic test to ensure the model can handle tensor inputs + try: + # Create a dummy input tensor (adjust size based on expected input) + dummy_input = torch.randn(1, 75) # Adjust dimensions as needed + + # Try to run inference (this might fail if input shape is wrong, but shouldn't crash) + with torch.no_grad(): + try: + output = model(dummy_input) + # If successful, check that output is a tensor + assert isinstance(output, torch.Tensor) + except (RuntimeError, ValueError): + # Expected if input shape doesn't match model expectations + # This is acceptable for this test + pass + except Exception: + # If model doesn't accept this input format, that's okay for this test + # The main goal is to ensure the model loads without crashing + pass + + +def test_load_torchscript_model_error_handling(): + """Test error handling when loading fails.""" + # Create a temporary file that will cause a loading error + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file.write(b"definitely not a torchscript model") + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) diff --git a/source/isaaclab/test/controllers/test_ik_configs/README.md b/source/isaaclab/test/controllers/test_ik_configs/README.md new file mode 100644 index 00000000000..ccbdae06b52 --- /dev/null +++ b/source/isaaclab/test/controllers/test_ik_configs/README.md @@ -0,0 +1,119 @@ +# Test Configuration Generation Guide + +This document explains how to generate test configurations for the Pink IK controller tests used in `test_pink_ik.py`. + +## File Structure + +Test configurations are JSON files with the following structure: + +```json +{ + "tolerances": { + "position": ..., + "pd_position": ..., + "rotation": ..., + "check_errors": true + }, + "allowed_steps_to_settle": ..., + "tests": { + "test_name": { + "left_hand_pose": [...], + "right_hand_pose": [...], + "allowed_steps_per_motion": ..., + "repeat": ... + } + } +} +``` + +## Parameters + +### Tolerances +- **position**: Maximum position error in meters +- **pd_position**: Maximum PD controller error in meters +- **rotation**: Maximum rotation error in radians +- **check_errors**: Whether to verify errors (should be `true`) + +### Test Parameters +- **allowed_steps_to_settle**: Initial settling steps (typically 100) +- **allowed_steps_per_motion**: Steps per motion phase +- **repeat**: Number of test repetitions +- **requires_waist_bending**: Whether the test requires waist bending (boolean) + +## Coordinate System + +### Robot Reset Pose +From `g1_locomanipulation_robot_cfg.py`: +- **Base position**: (0, 0, 0.75) - 75cm above ground +- **Base orientation**: 90° rotation around X-axis (facing forward) +- **Joint positions**: Standing pose with slight knee bend + +### EEF Pose Format +Each pose: `[x, y, z, qw, qx, qy, qz]` +- **Position**: Cartesian coordinates relative to robot base frame +- **Orientation**: Quaternion relative to the world. Typically you want this to start in the same orientation as robot base. (e.g. if robot base is reset to (0.7071, 0.0, 0.0, 0.7071), hand pose should be the same) + +**Note**: The system automatically compensates for hand rotational offsets, so specify orientations relative to the robot's reset orientation. + +## Creating Configurations + +### Step 1: Choose Robot Type +- `pink_ik_g1_test_configs.json` for G1 robot +- `pink_ik_gr1_test_configs.json` for GR1 robot + +### Step 2: Define Tolerances +```json +"tolerances": { + "position": 0.003, + "pd_position": 0.001, + "rotation": 0.017, + "check_errors": true +} +``` + +### Step 3: Create Test Movements +Common test types: +- **stay_still**: Same pose repeated +- **horizontal_movement**: Side-to-side movement +- **vertical_movement**: Up-and-down movement +- **rotation_movements**: Hand orientation changes + +### Step 4: Specify Hand Poses +```json +"horizontal_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 100, + "repeat": 2, + "requires_waist_bending": false +} +``` + +## Pose Guidelines + +### Orientation Examples +- **Default**: `[0.7071, 0.0, 0.0, 0.7071]` (90° around X-axis) +- **Z-rotation**: `[0.5, 0.0, 0.0, 0.866]` (60° around Z) +- **Y-rotation**: `[0.866, 0.0, 0.5, 0.0]` (60° around Y) + +## Testing Process + +1. Robot starts in reset pose and settles +2. Moves through each pose in sequence +3. Errors computed and verified against tolerances +4. Sequence repeats specified number of times + +### Waist Bending Logic +Tests marked with `"requires_waist_bending": true` will only run if waist joints are enabled in the environment configuration. The test system automatically detects waist capability by checking if waist joints (`waist_yaw_joint`, `waist_pitch_joint`, `waist_roll_joint`) are included in the `pink_controlled_joint_names` list. + +## Troubleshooting + +- **Can't reach target**: Check if within safe workspace +- **High errors**: Increase tolerances or adjust poses +- **Test failures**: Increase `allowed_steps_per_motion` diff --git a/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json new file mode 100644 index 00000000000..f5d0d60717d --- /dev/null +++ b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json @@ -0,0 +1,111 @@ +{ + "tolerances": { + "position": 0.003, + "pd_position": 0.002, + "rotation": 0.017, + "check_errors": true + }, + "allowed_steps_to_settle": 50, + "tests": { + "horizontal_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 15, + "repeat": 2, + "requires_waist_bending": false + }, + "horizontal_small_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.19, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.19, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 15, + "repeat": 2, + "requires_waist_bending": false + }, + "stay_still": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 20, + "repeat": 4, + "requires_waist_bending": false + }, + "vertical_movement": { + "left_hand_pose": [ + [-0.18, 0.15, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.9, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.15, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.9, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 30, + "repeat": 2, + "requires_waist_bending": false + }, + "forward_waist_bending_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.2, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.3, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.2, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.3, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 60, + "repeat": 2, + "requires_waist_bending": true + }, + "rotation_movements": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.2, 0.11, 0.8, 0.6946, 0.1325, 0.1325, 0.6946], + [-0.2, 0.11, 0.8, 0.6533, 0.2706, 0.2706, 0.6533], + [-0.2, 0.11, 0.8, 0.5848, 0.3975, 0.3975, 0.5848], + [-0.2, 0.11, 0.8, 0.5, 0.5, 0.5, 0.5], + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.2, 0.11, 0.8, 0.6946, -0.1325, -0.1325, 0.6946], + [-0.2, 0.11, 0.8, 0.6533, -0.2706, -0.2706, 0.6533], + [-0.2, 0.11, 0.8, 0.5848, -0.3975, -0.3975, 0.5848], + [-0.2, 0.11, 0.8, 0.5, -0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.2, 0.11, 0.8, 0.6946, -0.1325, -0.1325, 0.6946], + [0.2, 0.11, 0.8, 0.6533, -0.2706, -0.2706, 0.6533], + [0.2, 0.11, 0.8, 0.5848, -0.3975, -0.3975, 0.5848], + [0.2, 0.11, 0.8, 0.5, -0.5, -0.5, 0.5], + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.2, 0.11, 0.8, 0.6946, 0.1325, 0.1325, 0.6946], + [0.2, 0.11, 0.8, 0.6533, 0.2706, 0.2706, 0.6533], + [0.2, 0.11, 0.8, 0.5848, 0.3975, 0.3975, 0.5848], + [0.2, 0.11, 0.8, 0.5, 0.5, 0.5, 0.5] + ], + "allowed_steps_per_motion": 25, + "repeat": 2, + "requires_waist_bending": false + } + } +} diff --git a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json similarity index 76% rename from source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json rename to source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json index b033b95b81f..be40d7cf7ab 100644 --- a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json +++ b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json @@ -5,30 +5,33 @@ "rotation": 0.02, "check_errors": true }, + "allowed_steps_to_settle": 5, "tests": { - "stay_still": { + "vertical_movement": { "left_hand_pose": [ [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] + [-0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] ], "right_hand_pose": [ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] + [0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 10, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, - "vertical_movement": { + "stay_still": { "left_hand_pose": [ [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [-0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] + [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] ], "right_hand_pose": [ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5] + [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 15, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 4, + "requires_waist_bending": false }, "horizontal_movement": { "left_hand_pose": [ @@ -39,8 +42,9 @@ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.13, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 15, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, "horizontal_small_movement": { "left_hand_pose": [ @@ -51,8 +55,9 @@ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.22, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 15, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, "forward_waist_bending_movement": { "left_hand_pose": [ @@ -63,24 +68,26 @@ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5] ], - "allowed_steps_per_motion": 30, - "repeat": 3 + "allowed_steps_per_motion": 25, + "repeat": 3, + "requires_waist_bending": true }, "rotation_movements": { "left_hand_pose": [ [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [-0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0], [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [-0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071] + [-0.23, 0.32, 1.1, 0.0, 0.0, -0.7071, 0.7071] ], "right_hand_pose": [ [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], - [0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071], + [0.23, 0.32, 1.1, 0.0, 0.0, -0.7071, 0.7071], [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5], [0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0] ], - "allowed_steps_per_motion": 20, - "repeat": 2 + "allowed_steps_per_motion": 10, + "repeat": 2, + "requires_waist_bending": false } } } diff --git a/source/isaaclab/test/controllers/test_local_frame_task.py b/source/isaaclab/test/controllers/test_local_frame_task.py new file mode 100644 index 00000000000..48c86eec082 --- /dev/null +++ b/source/isaaclab/test/controllers/test_local_frame_task.py @@ -0,0 +1,481 @@ +# Copyright (c) 2022-2025, 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 cases for LocalFrameTask class.""" +# Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim +# pinocchio is required by the Pink IK controller +import sys + +if sys.platform != "win32": + import pinocchio # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import numpy as np +from pathlib import Path + +import pinocchio as pin +import pytest + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.pink_kinematics_configuration import PinkKinematicsConfiguration + +# class TestLocalFrameTask: +# """Test suite for LocalFrameTask class.""" + + +@pytest.fixture +def urdf_path(): + """Path to test URDF file.""" + return Path(__file__).parent / "urdfs" / "test_urdf_two_link_robot.urdf" + + +@pytest.fixture +def controlled_joint_names(): + """List of controlled joint names for testing.""" + return ["joint_1", "joint_2"] + + +@pytest.fixture +def pink_config(urdf_path, controlled_joint_names): + """Create a PinkKinematicsConfiguration instance for testing.""" + return PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + controlled_joint_names=controlled_joint_names, + # copy_data=True, + # forward_kinematics=True, + ) + + +@pytest.fixture +def local_frame_task(): + """Create a LocalFrameTask instance for testing.""" + return LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=0.0, + gain=1.0, + ) + + +def test_initialization(local_frame_task): + """Test proper initialization of LocalFrameTask.""" + # Check that the task is properly initialized + assert local_frame_task.frame == "link_2" + assert local_frame_task.base_link_frame_name == "base_link" + assert np.allclose(local_frame_task.cost[:3], [1.0, 1.0, 1.0]) + assert np.allclose(local_frame_task.cost[3:], [1.0, 1.0, 1.0]) + assert local_frame_task.lm_damping == 0.0 + assert local_frame_task.gain == 1.0 + + # Check that target is initially None + assert local_frame_task.transform_target_to_base is None + + +def test_initialization_with_sequence_costs(): + """Test initialization with sequence costs.""" + task = LocalFrameTask( + frame="link_1", + base_link_frame_name="base_link", + position_cost=[1.0, 1.0, 1.0], + orientation_cost=[1.0, 1.0, 1.0], + lm_damping=0.1, + gain=2.0, + ) + + assert task.frame == "link_1" + assert task.base_link_frame_name == "base_link" + assert np.allclose(task.cost[:3], [1.0, 1.0, 1.0]) + assert np.allclose(task.cost[3:], [1.0, 1.0, 1.0]) + assert task.lm_damping == 0.1 + assert task.gain == 2.0 + + +def test_inheritance_from_frame_task(local_frame_task): + """Test that LocalFrameTask properly inherits from FrameTask.""" + from pink.tasks.frame_task import FrameTask + + # Check inheritance + assert isinstance(local_frame_task, FrameTask) + + # Check that we can call parent class methods + assert hasattr(local_frame_task, "compute_error") + assert hasattr(local_frame_task, "compute_jacobian") + + +def test_set_target(local_frame_task): + """Test setting target with a transform.""" + # Create a test transform + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + target_transform.rotation = pin.exp3(np.array([0.1, 0.0, 0.0])) + + # Set the target + local_frame_task.set_target(target_transform) + + # Check that target was set correctly + assert local_frame_task.transform_target_to_base is not None + assert isinstance(local_frame_task.transform_target_to_base, pin.SE3) + + # Check that it's a copy (not the same object) + assert local_frame_task.transform_target_to_base is not target_transform + + # Check that values match + assert np.allclose(local_frame_task.transform_target_to_base.translation, target_transform.translation) + assert np.allclose(local_frame_task.transform_target_to_base.rotation, target_transform.rotation) + + +def test_set_target_from_configuration(local_frame_task, pink_config): + """Test setting target from a robot configuration.""" + # Set target from configuration + local_frame_task.set_target_from_configuration(pink_config) + + # Check that target was set + assert local_frame_task.transform_target_to_base is not None + assert isinstance(local_frame_task.transform_target_to_base, pin.SE3) + + +def test_set_target_from_configuration_wrong_type(local_frame_task): + """Test that set_target_from_configuration raises error with wrong type.""" + with pytest.raises(ValueError, match="configuration must be a PinkKinematicsConfiguration"): + local_frame_task.set_target_from_configuration("not_a_configuration") + + +def test_compute_error_with_target_set(local_frame_task, pink_config): + """Test computing error when target is set.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Check that error is computed correctly + assert isinstance(error, np.ndarray) + assert error.shape == (6,) # 6D error (3 position + 3 orientation) + + # Error should not be all zeros (unless target exactly matches current pose) + # This is a reasonable assumption for a random target + + +def test_compute_error_without_target(local_frame_task, pink_config): + """Test that compute_error raises error when no target is set.""" + with pytest.raises(ValueError, match="no target set for frame 'link_2'"): + local_frame_task.compute_error(pink_config) + + +def test_compute_error_wrong_configuration_type(local_frame_task): + """Test that compute_error raises error with wrong configuration type.""" + # Set a target first + target_transform = pin.SE3.Identity() + local_frame_task.set_target(target_transform) + + with pytest.raises(ValueError, match="configuration must be a PinkKinematicsConfiguration"): + local_frame_task.compute_error("not_a_configuration") + + +def test_compute_jacobian_with_target_set(local_frame_task, pink_config): + """Test computing Jacobian when target is set.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + + # Check that Jacobian is computed correctly + assert isinstance(jacobian, np.ndarray) + assert jacobian.shape == (6, 2) # 6 rows (error), 2 columns (controlled joints) + + # Jacobian should not be all zeros + assert not np.allclose(jacobian, 0.0) + + +def test_compute_jacobian_without_target(local_frame_task, pink_config): + """Test that compute_jacobian raises error when no target is set.""" + with pytest.raises(Exception, match="no target set for frame 'link_2'"): + local_frame_task.compute_jacobian(pink_config) + + +def test_error_consistency_across_configurations(local_frame_task, pink_config): + """Test that error computation is consistent across different configurations.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute error at initial configuration + error_1 = local_frame_task.compute_error(pink_config) + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.5 # Change first revolute joint + pink_config.update(new_q) + + # Compute error at new configuration + error_2 = local_frame_task.compute_error(pink_config) + + # Errors should be different (not all close) + assert not np.allclose(error_1, error_2) + + +def test_jacobian_consistency_across_configurations(local_frame_task, pink_config): + """Test that Jacobian computation is consistent across different configurations.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian at initial configuration + jacobian_1 = local_frame_task.compute_jacobian(pink_config) + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.3 # Change first revolute joint + pink_config.update(new_q) + + # Compute Jacobian at new configuration + jacobian_2 = local_frame_task.compute_jacobian(pink_config) + + # Jacobians should be different (not all close) + assert not np.allclose(jacobian_1, jacobian_2) + + +def test_error_zero_at_target_pose(local_frame_task, pink_config): + """Test that error is zero when current pose matches target pose.""" + # Get current transform of the frame + current_transform = pink_config.get_transform_frame_to_world("link_2") + + # Set target to current pose + local_frame_task.set_target(current_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Error should be very close to zero + assert np.allclose(error, 0.0, atol=1e-10) + + +def test_different_frames(pink_config): + """Test LocalFrameTask with different frame names.""" + # Test with link_1 frame + task_link1 = LocalFrameTask( + frame="link_1", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + # Set target and compute error + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.0, 0.0]) + task_link1.set_target(target_transform) + + error_link1 = task_link1.compute_error(pink_config) + assert error_link1.shape == (6,) + + # Test with base_link frame + task_base = LocalFrameTask( + frame="base_link", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + task_base.set_target(target_transform) + error_base = task_base.compute_error(pink_config) + assert error_base.shape == (6,) + + +def test_different_base_frames(pink_config): + """Test LocalFrameTask with different base frame names.""" + # Test with base_link as base frame + task_base_base = LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + target_transform = pin.SE3.Identity() + task_base_base.set_target(target_transform) + error_base_base = task_base_base.compute_error(pink_config) + assert error_base_base.shape == (6,) + + # Test with link_1 as base frame + task_link1_base = LocalFrameTask( + frame="link_2", + base_link_frame_name="link_1", + position_cost=1.0, + orientation_cost=1.0, + ) + + task_link1_base.set_target(target_transform) + error_link1_base = task_link1_base.compute_error(pink_config) + assert error_link1_base.shape == (6,) + + +def test_sequence_cost_parameters(): + """Test LocalFrameTask with sequence cost parameters.""" + task = LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=[1.0, 2.0, 3.0], + orientation_cost=[0.5, 1.0, 1.5], + lm_damping=0.1, + gain=2.0, + ) + + assert np.allclose(task.cost[:3], [1.0, 2.0, 3.0]) # Position costs + assert np.allclose(task.cost[3:], [0.5, 1.0, 1.5]) # Orientation costs + assert task.lm_damping == 0.1 + assert task.gain == 2.0 + + +def test_error_magnitude_consistency(local_frame_task, pink_config): + """Test that error computation produces reasonable results.""" + # Set a small target offset + small_target = pin.SE3.Identity() + small_target.translation = np.array([0.01, 0.01, 0.01]) + local_frame_task.set_target(small_target) + + error_small = local_frame_task.compute_error(pink_config) + + # Set a large target offset + large_target = pin.SE3.Identity() + large_target.translation = np.array([0.5, 0.5, 0.5]) + local_frame_task.set_target(large_target) + + error_large = local_frame_task.compute_error(pink_config) + + # Both errors should be finite and reasonable + assert np.all(np.isfinite(error_small)) + assert np.all(np.isfinite(error_large)) + assert not np.allclose(error_small, error_large) # Different targets should produce different errors + + +def test_jacobian_structure(local_frame_task, pink_config): + """Test that Jacobian has the correct structure.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + + # Check structure + assert jacobian.shape == (6, 2) # 6 error dimensions, 2 controlled joints + + # Check that Jacobian is not all zeros (basic functionality check) + assert not np.allclose(jacobian, 0.0) + + +def test_multiple_target_updates(local_frame_task, pink_config): + """Test that multiple target updates work correctly.""" + # Set first target + target1 = pin.SE3.Identity() + target1.translation = np.array([0.1, 0.0, 0.0]) + local_frame_task.set_target(target1) + + error1 = local_frame_task.compute_error(pink_config) + + # Set second target + target2 = pin.SE3.Identity() + target2.translation = np.array([0.0, 0.1, 0.0]) + local_frame_task.set_target(target2) + + error2 = local_frame_task.compute_error(pink_config) + + # Errors should be different + assert not np.allclose(error1, error2) + + +def test_inheritance_behavior(local_frame_task): + """Test that LocalFrameTask properly overrides parent class methods.""" + # Check that the class has the expected methods + assert hasattr(local_frame_task, "set_target") + assert hasattr(local_frame_task, "set_target_from_configuration") + assert hasattr(local_frame_task, "compute_error") + assert hasattr(local_frame_task, "compute_jacobian") + + # Check that these are the overridden methods, not the parent ones + assert local_frame_task.set_target.__qualname__ == "LocalFrameTask.set_target" + assert local_frame_task.compute_error.__qualname__ == "LocalFrameTask.compute_error" + assert local_frame_task.compute_jacobian.__qualname__ == "LocalFrameTask.compute_jacobian" + + +def test_target_copying_behavior(local_frame_task): + """Test that target transforms are properly copied.""" + # Create a target transform + original_target = pin.SE3.Identity() + original_target.translation = np.array([0.1, 0.2, 0.3]) + original_rotation = original_target.rotation.copy() + + # Set the target + local_frame_task.set_target(original_target) + + # Modify the original target + original_target.translation = np.array([0.5, 0.5, 0.5]) + original_target.rotation = pin.exp3(np.array([0.5, 0.0, 0.0])) + + # Check that the stored target is unchanged + assert np.allclose(local_frame_task.transform_target_to_base.translation, np.array([0.1, 0.2, 0.3])) + assert np.allclose(local_frame_task.transform_target_to_base.rotation, original_rotation) + + +def test_error_computation_with_orientation_difference(local_frame_task, pink_config): + """Test error computation when there's an orientation difference.""" + # Set a target with orientation difference + target_transform = pin.SE3.Identity() + target_transform.rotation = pin.exp3(np.array([0.2, 0.0, 0.0])) # Rotation around X-axis + local_frame_task.set_target(target_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Check that error is computed correctly + assert isinstance(error, np.ndarray) + assert error.shape == (6,) + + # Error should not be all zeros + assert not np.allclose(error, 0.0) + + +def test_jacobian_rank_consistency(local_frame_task, pink_config): + """Test that Jacobian maintains consistent shape across configurations.""" + # Set a target that we know can be reached by the test robot. + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.0, 0.0, 0.45]) + # 90 degrees around x axis = pi/2 radians + target_transform.rotation = pin.exp3(np.array([np.pi / 2, 0.0, 0.0])) + local_frame_task.set_target(target_transform) + + # Compute Jacobian at multiple configurations + jacobians = [] + for i in range(5): + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.1 * i # Vary first joint + pink_config.update(new_q) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + jacobians.append(jacobian) + + # All Jacobians should have the same shape + for jacobian in jacobians: + assert jacobian.shape == (6, 2) + + # All Jacobians should have rank 2 (full rank for 2-DOF planar arm) + for jacobian in jacobians: + assert np.linalg.matrix_rank(jacobian) == 2 diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index 3485f367e37..46f610c42f5 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -22,43 +22,55 @@ import gymnasium as gym import json import numpy as np -import os +import re import torch +from pathlib import Path +import omni.usd import pytest from pink.configuration import Configuration +from pink.tasks import FrameTask from isaaclab.utils.math import axis_angle_from_quat, matrix_from_quat, quat_from_matrix, quat_inv import isaaclab_tasks # noqa: F401 +import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -@pytest.fixture(scope="module") -def test_cfg(): - """Load test configuration.""" - config_path = os.path.join(os.path.dirname(__file__), "test_configs", "pink_ik_gr1_test_configs.json") +def load_test_config(env_name): + """Load test configuration based on environment type.""" + # Determine which config file to load based on environment name + if "G1" in env_name: + config_file = "pink_ik_g1_test_configs.json" + elif "GR1" in env_name: + config_file = "pink_ik_gr1_test_configs.json" + else: + raise ValueError(f"Unknown environment type in {env_name}. Expected G1 or GR1.") + + config_path = Path(__file__).parent / "test_ik_configs" / config_file with open(config_path) as f: return json.load(f) -@pytest.fixture(scope="module") -def test_params(test_cfg): - """Set up test parameters.""" - return { - "position_tolerance": test_cfg["tolerances"]["position"], - "rotation_tolerance": test_cfg["tolerances"]["rotation"], - "pd_position_tolerance": test_cfg["tolerances"]["pd_position"], - "check_errors": test_cfg["tolerances"]["check_errors"], - } +def is_waist_enabled(env_cfg): + """Check if waist joints are enabled in the environment configuration.""" + if not hasattr(env_cfg.actions, "upper_body_ik"): + return False + + pink_controlled_joints = env_cfg.actions.upper_body_ik.pink_controlled_joint_names + + # Also check for pattern-based joint names (e.g., "waist_.*_joint") + return any(re.match("waist", joint) for joint in pink_controlled_joints) -def create_test_env(num_envs): +def create_test_env(env_name, num_envs): """Create a test environment with the Pink IK controller.""" - env_name = "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0" device = "cuda:0" + omni.usd.get_context().new_stage() + try: env_cfg = parse_env_cfg(env_name, device=device, num_envs=num_envs) # Modify scene config to not spawn the packing table to avoid collision with the robot @@ -71,85 +83,133 @@ def create_test_env(num_envs): raise -@pytest.fixture(scope="module") -def env_and_cfg(): +@pytest.fixture( + scope="module", + params=[ + "Isaac-PickPlace-GR1T2-Abs-v0", + "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0", + "Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0", + "Isaac-PickPlace-Locomanipulation-G1-Abs-v0", + ], +) +def env_and_cfg(request): """Create environment and configuration for tests.""" - env, env_cfg = create_test_env(num_envs=1) + env_name = request.param + + # Load the appropriate test configuration based on environment type + test_cfg = load_test_config(env_name) + + env, env_cfg = create_test_env(env_name, num_envs=1) + + # Get only the FrameTasks from variable_input_tasks + variable_input_tasks = [ + task for task in env_cfg.actions.upper_body_ik.controller.variable_input_tasks if isinstance(task, FrameTask) + ] + assert len(variable_input_tasks) == 2, "Expected exactly two FrameTasks (left and right hand)." + frames = [task.frame for task in variable_input_tasks] + # Try to infer which is left and which is right + left_candidates = [f for f in frames if "left" in f.lower()] + right_candidates = [f for f in frames if "right" in f.lower()] + assert ( + len(left_candidates) == 1 and len(right_candidates) == 1 + ), f"Could not uniquely identify left/right frames from: {frames}" + left_eef_urdf_link_name = left_candidates[0] + right_eef_urdf_link_name = right_candidates[0] # Set up camera view env.sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 1.0]) - return env, env_cfg + # Create test parameters from test_cfg + test_params = { + "position": test_cfg["tolerances"]["position"], + "rotation": test_cfg["tolerances"]["rotation"], + "pd_position": test_cfg["tolerances"]["pd_position"], + "check_errors": test_cfg["tolerances"]["check_errors"], + "left_eef_urdf_link_name": left_eef_urdf_link_name, + "right_eef_urdf_link_name": right_eef_urdf_link_name, + } + + try: + yield env, env_cfg, test_cfg, test_params + finally: + env.close() @pytest.fixture def test_setup(env_and_cfg): """Set up test case - runs before each test.""" - env, env_cfg = env_and_cfg + env, env_cfg, test_cfg, test_params = env_and_cfg - num_joints_in_robot_hands = env_cfg.actions.pink_ik_cfg.controller.num_hand_joints + num_joints_in_robot_hands = env_cfg.actions.upper_body_ik.controller.num_hand_joints # Get Action Term and IK controller - action_term = env.action_manager.get_term(name="pink_ik_cfg") + action_term = env.action_manager.get_term(name="upper_body_ik") pink_controllers = action_term._ik_controllers articulation = action_term._asset # Initialize Pink Configuration for forward kinematics - kinematics_model = Configuration( - pink_controllers[0].robot_wrapper.model, - pink_controllers[0].robot_wrapper.data, - pink_controllers[0].robot_wrapper.q0, + test_kinematics_model = Configuration( + pink_controllers[0].pink_configuration.model, + pink_controllers[0].pink_configuration.data, + pink_controllers[0].pink_configuration.q, ) - left_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["left_wrist"] - right_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["right_wrist"] + left_target_link_name = env_cfg.actions.upper_body_ik.target_eef_link_names["left_wrist"] + right_target_link_name = env_cfg.actions.upper_body_ik.target_eef_link_names["right_wrist"] return { "env": env, "env_cfg": env_cfg, + "test_cfg": test_cfg, + "test_params": test_params, "num_joints_in_robot_hands": num_joints_in_robot_hands, "action_term": action_term, "pink_controllers": pink_controllers, "articulation": articulation, - "kinematics_model": kinematics_model, + "test_kinematics_model": test_kinematics_model, "left_target_link_name": left_target_link_name, "right_target_link_name": right_target_link_name, + "left_eef_urdf_link_name": test_params["left_eef_urdf_link_name"], + "right_eef_urdf_link_name": test_params["right_eef_urdf_link_name"], } -def test_stay_still(test_setup, test_cfg): - """Test staying still.""" - print("Running stay still test...") - run_movement_test(test_setup, test_cfg["tests"]["stay_still"], test_cfg) - - -def test_vertical_movement(test_setup, test_cfg): - """Test vertical movement of robot hands.""" - print("Running vertical movement test...") - run_movement_test(test_setup, test_cfg["tests"]["vertical_movement"], test_cfg) - - -def test_horizontal_movement(test_setup, test_cfg): - """Test horizontal movement of robot hands.""" - print("Running horizontal movement test...") - run_movement_test(test_setup, test_cfg["tests"]["horizontal_movement"], test_cfg) - - -def test_horizontal_small_movement(test_setup, test_cfg): - """Test small horizontal movement of robot hands.""" - print("Running horizontal small movement test...") - run_movement_test(test_setup, test_cfg["tests"]["horizontal_small_movement"], test_cfg) - - -def test_forward_waist_bending_movement(test_setup, test_cfg): - """Test forward waist bending movement of robot hands.""" - print("Running forward waist bending movement test...") - run_movement_test(test_setup, test_cfg["tests"]["forward_waist_bending_movement"], test_cfg) - +@pytest.mark.parametrize( + "test_name", + [ + "horizontal_movement", + "horizontal_small_movement", + "stay_still", + "forward_waist_bending_movement", + "vertical_movement", + "rotation_movements", + ], +) +def test_movement_types(test_setup, test_name): + """Test different movement types using parametrization.""" + test_cfg = test_setup["test_cfg"] + env_cfg = test_setup["env_cfg"] + + if test_name not in test_cfg["tests"]: + print(f"Skipping {test_name} test for {env_cfg.__class__.__name__} environment (test not defined)...") + pytest.skip(f"Test {test_name} not defined for {env_cfg.__class__.__name__}") + return + + test_config = test_cfg["tests"][test_name] + + # Check if test requires waist bending and if waist is enabled + requires_waist_bending = test_config.get("requires_waist_bending", False) + waist_enabled = is_waist_enabled(env_cfg) + + if requires_waist_bending and not waist_enabled: + print( + f"Skipping {test_name} test because it requires waist bending but waist is not enabled in" + f" {env_cfg.__class__.__name__}..." + ) + pytest.skip(f"Test {test_name} requires waist bending but waist is not enabled") + return -def test_rotation_movements(test_setup, test_cfg): - """Test rotation movements of robot hands.""" - print("Running rotation movements test...") - run_movement_test(test_setup, test_cfg["tests"]["rotation_movements"], test_cfg) + print(f"Running {test_name} test...") + run_movement_test(test_setup, test_config, test_cfg) def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): @@ -167,8 +227,14 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): obs, _ = env.reset() + # Make the first phase longer than subsequent ones + initial_steps = test_cfg["allowed_steps_to_settle"] + phase = "initial" + steps_in_phase = 0 + while simulation_app.is_running() and not simulation_app.is_exiting(): num_runs += 1 + steps_in_phase += 1 # Call auxiliary function if provided if aux_function is not None: @@ -178,20 +244,40 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): setpoint_poses = np.concatenate([left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx]]) actions = np.concatenate([setpoint_poses, np.zeros(num_joints_in_robot_hands)]) actions = torch.tensor(actions, device=env.device, dtype=torch.float32) + # Append base command for Locomanipulation environments with fixed height + if test_setup["env_cfg"].__class__.__name__ == "LocomanipulationG1EnvCfg": + # Use a named variable for base height for clarity and maintainability + BASE_HEIGHT = 0.72 + base_command = torch.zeros(4, device=env.device, dtype=actions.dtype) + base_command[3] = BASE_HEIGHT + actions = torch.cat([actions, base_command]) actions = actions.repeat(env.num_envs, 1) # Step environment obs, _, _, _, _ = env.step(actions) + # Determine the step interval for error checking + if phase == "initial": + check_interval = initial_steps + else: + check_interval = test_config["allowed_steps_per_motion"] + # Check convergence and verify errors - if num_runs % test_config["allowed_steps_per_motion"] == 0: + if steps_in_phase % check_interval == 0: print("Computing errors...") errors = compute_errors( - test_setup, env, left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx] + test_setup, + env, + left_hand_poses[curr_pose_idx], + right_hand_poses[curr_pose_idx], + test_setup["left_eef_urdf_link_name"], + test_setup["right_eef_urdf_link_name"], ) print_debug_info(errors, test_counter) - if test_cfg["tolerances"]["check_errors"]: - verify_errors(errors, test_setup, test_cfg["tolerances"]) + test_params = test_setup["test_params"] + if test_params["check_errors"]: + verify_errors(errors, test_setup, test_params) + num_runs += 1 curr_pose_idx = (curr_pose_idx + 1) % len(left_hand_poses) if curr_pose_idx == 0: @@ -199,6 +285,10 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): if test_counter > test_config["repeat"]: print("Test completed successfully") break + # After the first phase, switch to normal interval + if phase == "initial": + phase = "normal" + steps_in_phase = 0 def get_link_pose(env, link_name): @@ -225,15 +315,16 @@ def calculate_rotation_error(current_rot, target_rot): ) -def compute_errors(test_setup, env, left_target_pose, right_target_pose): +def compute_errors( + test_setup, env, left_target_pose, right_target_pose, left_eef_urdf_link_name, right_eef_urdf_link_name +): """Compute all error metrics for the current state.""" action_term = test_setup["action_term"] pink_controllers = test_setup["pink_controllers"] articulation = test_setup["articulation"] - kinematics_model = test_setup["kinematics_model"] + test_kinematics_model = test_setup["test_kinematics_model"] left_target_link_name = test_setup["left_target_link_name"] right_target_link_name = test_setup["right_target_link_name"] - num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"] # Get current hand positions and orientations left_hand_pos, left_hand_rot = get_link_pose(env, left_target_link_name) @@ -244,10 +335,6 @@ def compute_errors(test_setup, env, left_target_pose, right_target_pose): num_envs = env.num_envs left_hand_pose_setpoint = torch.tensor(left_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) right_hand_pose_setpoint = torch.tensor(right_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1) - # compensate for the hand rotational offset - # nominal_hand_pose_rotmat = matrix_from_quat(torch.tensor(env_cfg.actions.pink_ik_cfg.controller.hand_rotational_offset, device=env.device)) - left_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(left_hand_pose_setpoint[:, 3:7])) - right_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(right_hand_pose_setpoint[:, 3:7])) # Calculate position and rotation errors left_pos_error = left_hand_pose_setpoint[:, :3] - left_hand_pos @@ -257,32 +344,24 @@ def compute_errors(test_setup, env, left_target_pose, right_target_pose): # Calculate PD controller errors ik_controller = pink_controllers[0] - pink_controlled_joint_ids = action_term._pink_controlled_joint_ids + isaaclab_controlled_joint_ids = action_term._isaaclab_controlled_joint_ids - # Get current and target positions - curr_joints = articulation.data.joint_pos[:, pink_controlled_joint_ids].cpu().numpy()[0] - target_joints = action_term.processed_actions[:, :num_joints_in_robot_hands].cpu().numpy()[0] + # Get current and target positions for controlled joints only + curr_joints = articulation.data.joint_pos[:, isaaclab_controlled_joint_ids].cpu().numpy()[0] + target_joints = action_term.processed_actions[:, : len(isaaclab_controlled_joint_ids)].cpu().numpy()[0] - # Reorder joints for Pink IK - curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_ordering] - target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_ordering] + # Reorder joints for Pink IK (using controlled joint ordering) + curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_controlled_ordering] + target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_controlled_ordering] # Run forward kinematics - kinematics_model.update(curr_joints) - left_curr_pos = kinematics_model.get_transform_frame_to_world( - frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link" - ).translation - right_curr_pos = kinematics_model.get_transform_frame_to_world( - frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link" - ).translation - - kinematics_model.update(target_joints) - left_target_pos = kinematics_model.get_transform_frame_to_world( - frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link" - ).translation - right_target_pos = kinematics_model.get_transform_frame_to_world( - frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link" - ).translation + test_kinematics_model.update(curr_joints) + left_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation + right_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation + + test_kinematics_model.update(target_joints) + left_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation + right_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation # Calculate PD errors left_pd_error = ( diff --git a/source/isaaclab/test/controllers/test_pink_ik_components.py b/source/isaaclab/test/controllers/test_pink_ik_components.py new file mode 100644 index 00000000000..6a691c353b2 --- /dev/null +++ b/source/isaaclab/test/controllers/test_pink_ik_components.py @@ -0,0 +1,307 @@ +# Copyright (c) 2022-2025, 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 cases for PinkKinematicsConfiguration class.""" +# Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim +# pinocchio is required by the Pink IK controller +import sys + +if sys.platform != "win32": + import pinocchio # noqa: F401 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import numpy as np +from pathlib import Path + +import pinocchio as pin +import pytest +from pink.exceptions import FrameNotFound + +from isaaclab.controllers.pink_ik.pink_kinematics_configuration import PinkKinematicsConfiguration + + +class TestPinkKinematicsConfiguration: + """Test suite for PinkKinematicsConfiguration class.""" + + @pytest.fixture + def urdf_path(self): + """Path to test URDF file.""" + return Path(__file__).parent / "urdfs/test_urdf_two_link_robot.urdf" + + @pytest.fixture + def mesh_path(self): + """Path to mesh directory (empty for simple test).""" + return "" + + @pytest.fixture + def controlled_joint_names(self): + """List of controlled joint names for testing.""" + return ["joint_1", "joint_2"] + + @pytest.fixture + def pink_config(self, urdf_path, mesh_path, controlled_joint_names): + """Create a PinkKinematicsConfiguration instance for testing.""" + return PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + copy_data=True, + forward_kinematics=True, + ) + + def test_initialization(self, pink_config, controlled_joint_names): + """Test proper initialization of PinkKinematicsConfiguration.""" + # Check that controlled joint names are stored correctly + assert pink_config._controlled_joint_names == controlled_joint_names + + # Check that both full and controlled models are created + assert pink_config.full_model is not None + assert pink_config.controlled_model is not None + assert pink_config.full_data is not None + assert pink_config.controlled_data is not None + + # Check that configuration vectors are initialized + assert pink_config.full_q is not None + assert pink_config.controlled_q is not None + + # Check that the controlled model has the same number or fewer joints than the full model + assert pink_config.controlled_model.nq == pink_config.full_model.nq + + def test_joint_names_properties(self, pink_config): + """Test joint name properties.""" + # Test controlled joint names in pinocchio order + controlled_names = pink_config.controlled_joint_names_pinocchio_order + assert isinstance(controlled_names, list) + assert len(controlled_names) == len(pink_config._controlled_joint_names) + assert "joint_1" in controlled_names + assert "joint_2" in controlled_names + + # Test all joint names in pinocchio order + all_names = pink_config.all_joint_names_pinocchio_order + assert isinstance(all_names, list) + assert len(all_names) == len(controlled_names) + assert "joint_1" in all_names + assert "joint_2" in all_names + + def test_update_with_valid_configuration(self, pink_config): + """Test updating configuration with valid joint values.""" + # Get initial configuration + initial_q = pink_config.full_q.copy() + + # Create a new configuration with different joint values + new_q = initial_q.copy() + new_q[1] = 0.5 # Change first revolute joint value (index 1, since 0 is fixed joint) + + # Update configuration + pink_config.update(new_q) + + # Check that the configuration was updated + print(pink_config.full_q) + assert not np.allclose(pink_config.full_q, initial_q) + assert np.allclose(pink_config.full_q, new_q) + + def test_update_with_none(self, pink_config): + """Test updating configuration with None (should use current configuration).""" + # Get initial configuration + initial_q = pink_config.full_q.copy() + + # Update with None + pink_config.update(None) + + # Configuration should remain the same + assert np.allclose(pink_config.full_q, initial_q) + + def test_update_with_wrong_dimensions(self, pink_config): + """Test that update raises ValueError with wrong configuration dimensions.""" + # Create configuration with wrong number of joints + wrong_q = np.array([0.1, 0.2, 0.3]) # Wrong number of joints + + with pytest.raises(ValueError, match="q must have the same length as the number of joints"): + pink_config.update(wrong_q) + + def test_get_frame_jacobian_existing_frame(self, pink_config): + """Test getting Jacobian for an existing frame.""" + # Get Jacobian for link_1 frame + jacobian = pink_config.get_frame_jacobian("link_1") + + # Check that Jacobian has correct shape + # Should be 6 rows (linear + angular velocity) and columns equal to controlled joints + expected_rows = 6 + expected_cols = len(pink_config._controlled_joint_names) + assert jacobian.shape == (expected_rows, expected_cols) + + # Check that Jacobian is not all zeros (should have some non-zero values) + assert not np.allclose(jacobian, 0.0) + + def test_get_frame_jacobian_nonexistent_frame(self, pink_config): + """Test that get_frame_jacobian raises FrameNotFound for non-existent frame.""" + with pytest.raises(FrameNotFound): + pink_config.get_frame_jacobian("nonexistent_frame") + + def test_get_transform_frame_to_world_existing_frame(self, pink_config): + """Test getting transform for an existing frame.""" + # Get transform for link_1 frame + transform = pink_config.get_transform_frame_to_world("link_1") + + # Check that transform is a pinocchio SE3 object + assert isinstance(transform, pin.SE3) + + # Check that transform has reasonable values (not identity for non-zero joint angles) + assert not np.allclose(transform.homogeneous, np.eye(4)) + + def test_get_transform_frame_to_world_nonexistent_frame(self, pink_config): + """Test that get_transform_frame_to_world raises FrameNotFound for non-existent frame.""" + with pytest.raises(FrameNotFound): + pink_config.get_transform_frame_to_world("nonexistent_frame") + + def test_multiple_controlled_joints(self, urdf_path, mesh_path): + """Test configuration with multiple controlled joints.""" + # Create configuration with all available joints as controlled + controlled_joint_names = ["joint_1", "joint_2"] # Both revolute joints + + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + # Check that controlled model has correct number of joints + assert pink_config.controlled_model.nq == len(controlled_joint_names) + + def test_no_controlled_joints(self, urdf_path, mesh_path): + """Test configuration with no controlled joints.""" + controlled_joint_names = [] + + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + # Check that controlled model has 0 joints + assert pink_config.controlled_model.nq == 0 + assert len(pink_config.controlled_q) == 0 + + def test_jacobian_consistency(self, pink_config): + """Test that Jacobian computation is consistent across updates.""" + # Get Jacobian at initial configuration + jacobian_1 = pink_config.get_frame_jacobian("link_2") + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.3 # Change first revolute joint (index 1, since 0 is fixed joint) + pink_config.update(new_q) + + # Get Jacobian at new configuration + jacobian_2 = pink_config.get_frame_jacobian("link_2") + + # Jacobians should be different (not all close) + assert not np.allclose(jacobian_1, jacobian_2) + + def test_transform_consistency(self, pink_config): + """Test that transform computation is consistent across updates.""" + # Get transform at initial configuration + transform_1 = pink_config.get_transform_frame_to_world("link_2") + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.5 # Change first revolute joint (index 1, since 0 is fixed joint) + pink_config.update(new_q) + + # Get transform at new configuration + transform_2 = pink_config.get_transform_frame_to_world("link_2") + + # Transforms should be different + assert not np.allclose(transform_1.homogeneous, transform_2.homogeneous) + + def test_inheritance_from_configuration(self, pink_config): + """Test that PinkKinematicsConfiguration properly inherits from Pink Configuration.""" + from pink.configuration import Configuration + + # Check inheritance + assert isinstance(pink_config, Configuration) + + # Check that we can call parent class methods + assert hasattr(pink_config, "update") + assert hasattr(pink_config, "get_transform_frame_to_world") + + def test_controlled_joint_indices_calculation(self, pink_config): + """Test that controlled joint indices are calculated correctly.""" + # Check that controlled joint indices are valid + assert len(pink_config._controlled_joint_indices) == len(pink_config._controlled_joint_names) + + # Check that all indices are within bounds + for idx in pink_config._controlled_joint_indices: + assert 0 <= idx < len(pink_config._all_joint_names) + + # Check that indices correspond to controlled joint names + for i, idx in enumerate(pink_config._controlled_joint_indices): + joint_name = pink_config._all_joint_names[idx] + assert joint_name in pink_config._controlled_joint_names + + def test_full_model_integrity(self, pink_config): + """Test that the full model maintains integrity.""" + # Check that full model has all joints + assert pink_config.full_model.nq > 0 + assert len(pink_config.full_model.names) > 1 # More than just "universe" + + def test_controlled_model_integrity(self, pink_config): + """Test that the controlled model maintains integrity.""" + # Check that controlled model has correct number of joints + assert pink_config.controlled_model.nq == len(pink_config._controlled_joint_names) + + def test_configuration_vector_consistency(self, pink_config): + """Test that configuration vectors are consistent between full and controlled models.""" + # Check that controlled_q is a subset of full_q + controlled_indices = pink_config._controlled_joint_indices + for i, idx in enumerate(controlled_indices): + assert np.isclose(pink_config.controlled_q[i], pink_config.full_q[idx]) + + def test_error_handling_invalid_urdf(self, mesh_path, controlled_joint_names): + """Test error handling with invalid URDF path.""" + with pytest.raises(Exception): # Should raise some exception for invalid URDF + PinkKinematicsConfiguration( + urdf_path="nonexistent.urdf", + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + def test_error_handling_invalid_joint_names(self, urdf_path, mesh_path): + """Test error handling with invalid joint names.""" + invalid_joint_names = ["nonexistent_joint"] + + # This should not raise an error, but the controlled model should have 0 joints + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=invalid_joint_names, + ) + + assert pink_config.controlled_model.nq == 0 + assert len(pink_config.controlled_q) == 0 + + def test_undercontrolled_kinematics_model(self, urdf_path, mesh_path): + """Test that the fixed joint to world is properly handled.""" + + test_model = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=["joint_1"], + copy_data=True, + forward_kinematics=True, + ) + # Check that the controlled model only includes the revolute joints + assert "joint_1" in test_model.controlled_joint_names_pinocchio_order + assert "joint_2" not in test_model.controlled_joint_names_pinocchio_order + assert len(test_model.controlled_joint_names_pinocchio_order) == 1 # Only the two revolute joints + + # Check that the full configuration has more elements than controlled + assert len(test_model.full_q) > len(test_model.controlled_q) + assert len(test_model.full_q) == len(test_model.all_joint_names_pinocchio_order) + assert len(test_model.controlled_q) == len(test_model.controlled_joint_names_pinocchio_order) diff --git a/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf b/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf new file mode 100644 index 00000000000..cb1a305c50d --- /dev/null +++ b/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab/test/envs/test_manager_based_rl_env_obs_spaces.py b/source/isaaclab/test/envs/test_manager_based_rl_env_obs_spaces.py index 4f1b364e1e7..d8a8e8e32be 100644 --- a/source/isaaclab/test/envs/test_manager_based_rl_env_obs_spaces.py +++ b/source/isaaclab/test/envs/test_manager_based_rl_env_obs_spaces.py @@ -26,38 +26,6 @@ from isaaclab_tasks.manager_based.locomotion.velocity.config.anymal_c.rough_env_cfg import AnymalCRoughEnvCfg -@pytest.mark.parametrize( - "env_cfg_cls", - [CartpoleRGBCameraEnvCfg, CartpoleDepthCameraEnvCfg, AnymalCRoughEnvCfg], - ids=["RGB", "Depth", "RayCaster"], -) -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_obs_space_follows_clip_contraint(env_cfg_cls, device): - """Ensure curriculum terms apply correctly after the fallback and replacement.""" - # new USD stage - omni.usd.get_context().new_stage() - - # configure the cartpole env - env_cfg = env_cfg_cls() - env_cfg.scene.num_envs = 2 # keep num_envs small for testing - env_cfg.observations.policy.concatenate_terms = False - env_cfg.sim.device = device - - env = ManagerBasedRLEnv(cfg=env_cfg) - for group_name, group_space in env.observation_space.spaces.items(): - for term_name, term_space in group_space.spaces.items(): - term_cfg = getattr(getattr(env_cfg.observations, group_name), term_name) - low = -np.inf if term_cfg.clip is None else term_cfg.clip[0] - high = np.inf if term_cfg.clip is None else term_cfg.clip[1] - assert isinstance( - term_space, gym.spaces.Box - ), f"Expected Box space for {term_name} in {group_name}, got {type(term_space)}" - assert np.all(term_space.low == low) - assert np.all(term_space.high == high) - - env.close() - - @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_non_concatenated_obs_groups_contain_all_terms(device): """Test that non-concatenated observation groups contain all defined terms (issue #3133). @@ -139,3 +107,35 @@ def test_non_concatenated_obs_groups_contain_all_terms(device): assert term_name in obs["subtask_terms"], f"Term '{term_name}' missing from subtask_terms observation" env.close() + + +@pytest.mark.parametrize( + "env_cfg_cls", + [CartpoleRGBCameraEnvCfg, CartpoleDepthCameraEnvCfg, AnymalCRoughEnvCfg], + ids=["RGB", "Depth", "RayCaster"], +) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_obs_space_follows_clip_contraint(env_cfg_cls, device): + """Ensure curriculum terms apply correctly after the fallback and replacement.""" + # new USD stage + omni.usd.get_context().new_stage() + + # configure the cartpole env + env_cfg = env_cfg_cls() + env_cfg.scene.num_envs = 2 # keep num_envs small for testing + env_cfg.observations.policy.concatenate_terms = False + env_cfg.sim.device = device + + env = ManagerBasedRLEnv(cfg=env_cfg) + for group_name, group_space in env.observation_space.spaces.items(): + for term_name, term_space in group_space.spaces.items(): + term_cfg = getattr(getattr(env_cfg.observations, group_name), term_name) + low = -np.inf if term_cfg.clip is None else term_cfg.clip[0] + high = np.inf if term_cfg.clip is None else term_cfg.clip[1] + assert isinstance( + term_space, gym.spaces.Box + ), f"Expected Box space for {term_name} in {group_name}, got {type(term_space)}" + assert np.all(term_space.low == low) + assert np.all(term_space.high == high) + + env.close() diff --git a/source/isaaclab/test/performance/test_kit_startup_performance.py b/source/isaaclab/test/performance/test_kit_startup_performance.py index 056b2e6293b..dfa716cd0b2 100644 --- a/source/isaaclab/test/performance/test_kit_startup_performance.py +++ b/source/isaaclab/test/performance/test_kit_startup_performance.py @@ -10,12 +10,9 @@ import time -import pytest - from isaaclab.app import AppLauncher -@pytest.mark.isaacsim_ci def test_kit_start_up_time(): """Test kit start-up time.""" start_time = time.time() diff --git a/source/isaaclab/test/performance/test_robot_load_performance.py b/source/isaaclab/test/performance/test_robot_load_performance.py index 4acf8ad6331..bca8c36d9d5 100644 --- a/source/isaaclab/test/performance/test_robot_load_performance.py +++ b/source/isaaclab/test/performance/test_robot_load_performance.py @@ -33,7 +33,6 @@ ({"name": "Anymal_D", "robot_cfg": ANYMAL_D_CFG, "expected_load_time": 40.0}, "cpu"), ], ) -@pytest.mark.isaacsim_ci def test_robot_load_performance(test_config, device): """Test robot load time.""" with build_simulation_context(device=device) as sim: diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index c30a7d2eaf1..4512b29f3b2 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -223,6 +223,7 @@ def setup_simulation(): @pytest.mark.parametrize("disable_contact_processing", [True, False]) +@flaky(max_runs=3, min_passes=1) def test_cube_contact_time(setup_simulation, disable_contact_processing): """Checks contact sensor values for contact time and air time for a cube collision primitive.""" # check for both contact processing enabled and disabled diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index 2e107c60b2f..fdef7a3ae5c 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -40,7 +40,7 @@ @pytest.fixture(scope="function") -def setup_camera() -> tuple[sim_utils.SimulationContext, TiledCameraCfg, float]: +def setup_camera(device) -> tuple[sim_utils.SimulationContext, TiledCameraCfg, float]: """Fixture to set up and tear down the camera simulation environment.""" camera_cfg = TiledCameraCfg( height=128, @@ -58,7 +58,7 @@ def setup_camera() -> tuple[sim_utils.SimulationContext, TiledCameraCfg, float]: # Simulation time-step dt = 0.01 # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim_cfg = sim_utils.SimulationCfg(dt=dt, device=device) sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg) # populate scene _populate_scene() @@ -72,8 +72,9 @@ def setup_camera() -> tuple[sim_utils.SimulationContext, TiledCameraCfg, float]: sim.clear_instance() +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_single_camera_init(setup_camera): +def test_single_camera_init(setup_camera, device): """Test single camera initialization.""" sim, camera_cfg, dt = setup_camera # Create camera @@ -119,8 +120,9 @@ def test_single_camera_init(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_depth_clipping_max(setup_camera): +def test_depth_clipping_max(setup_camera, device): """Test depth max clipping.""" sim, _, dt = setup_camera # get camera cfgs @@ -158,8 +160,9 @@ def test_depth_clipping_max(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_depth_clipping_none(setup_camera): +def test_depth_clipping_none(setup_camera, device): """Test depth none clipping.""" sim, _, dt = setup_camera # get camera cfgs @@ -201,8 +204,9 @@ def test_depth_clipping_none(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_depth_clipping_zero(setup_camera): +def test_depth_clipping_zero(setup_camera, device): """Test depth zero clipping.""" sim, _, dt = setup_camera # get camera cfgs @@ -240,8 +244,9 @@ def test_depth_clipping_zero(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_multi_camera_init(setup_camera): +def test_multi_camera_init(setup_camera, device): """Test multi-camera initialization.""" sim, camera_cfg, dt = setup_camera @@ -296,8 +301,9 @@ def test_multi_camera_init(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_rgb_only_camera(setup_camera): +def test_rgb_only_camera(setup_camera, device): """Test initialization with only RGB data type.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -349,8 +355,9 @@ def test_rgb_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_data_types(setup_camera): +def test_data_types(setup_camera, device): """Test different data types for camera initialization.""" sim, camera_cfg, dt = setup_camera # Create camera @@ -396,8 +403,9 @@ def test_data_types(setup_camera): del camera_both +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_depth_only_camera(setup_camera): +def test_depth_only_camera(setup_camera, device): """Test initialization with only depth.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -449,8 +457,9 @@ def test_depth_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_rgba_only_camera(setup_camera): +def test_rgba_only_camera(setup_camera, device): """Test initialization with only RGBA.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -502,8 +511,9 @@ def test_rgba_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_distance_to_camera_only_camera(setup_camera): +def test_distance_to_camera_only_camera(setup_camera, device): """Test initialization with only distance_to_camera.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -555,8 +565,9 @@ def test_distance_to_camera_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_distance_to_image_plane_only_camera(setup_camera): +def test_distance_to_image_plane_only_camera(setup_camera, device): """Test initialization with only distance_to_image_plane.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -608,8 +619,9 @@ def test_distance_to_image_plane_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_normals_only_camera(setup_camera): +def test_normals_only_camera(setup_camera, device): """Test initialization with only normals.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -661,8 +673,9 @@ def test_normals_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_motion_vectors_only_camera(setup_camera): +def test_motion_vectors_only_camera(setup_camera, device): """Test initialization with only motion_vectors.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -714,8 +727,9 @@ def test_motion_vectors_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_semantic_segmentation_colorize_only_camera(setup_camera): +def test_semantic_segmentation_colorize_only_camera(setup_camera, device): """Test initialization with only semantic_segmentation.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -768,8 +782,9 @@ def test_semantic_segmentation_colorize_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_instance_segmentation_fast_colorize_only_camera(setup_camera): +def test_instance_segmentation_fast_colorize_only_camera(setup_camera, device): """Test initialization with only instance_segmentation_fast.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -822,8 +837,9 @@ def test_instance_segmentation_fast_colorize_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_instance_id_segmentation_fast_colorize_only_camera(setup_camera): +def test_instance_id_segmentation_fast_colorize_only_camera(setup_camera, device): """Test initialization with only instance_id_segmentation_fast.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -876,8 +892,9 @@ def test_instance_id_segmentation_fast_colorize_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_semantic_segmentation_non_colorize_only_camera(setup_camera): +def test_semantic_segmentation_non_colorize_only_camera(setup_camera, device): """Test initialization with only semantic_segmentation.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -932,8 +949,9 @@ def test_semantic_segmentation_non_colorize_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_instance_segmentation_fast_non_colorize_only_camera(setup_camera): +def test_instance_segmentation_fast_non_colorize_only_camera(setup_camera, device): """Test initialization with only instance_segmentation_fast.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -987,7 +1005,8 @@ def test_instance_segmentation_fast_non_colorize_only_camera(setup_camera): del camera -def test_instance_id_segmentation_fast_non_colorize_only_camera(setup_camera): +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_instance_id_segmentation_fast_non_colorize_only_camera(setup_camera, device): """Test initialization with only instance_id_segmentation_fast.""" sim, camera_cfg, dt = setup_camera num_cameras = 9 @@ -1041,8 +1060,9 @@ def test_instance_id_segmentation_fast_non_colorize_only_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_all_annotators_camera(setup_camera): +def test_all_annotators_camera(setup_camera, device): """Test initialization with all supported annotators.""" sim, camera_cfg, dt = setup_camera all_annotator_types = [ @@ -1140,8 +1160,9 @@ def test_all_annotators_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_all_annotators_low_resolution_camera(setup_camera): +def test_all_annotators_low_resolution_camera(setup_camera, device): """Test initialization with all supported annotators.""" sim, camera_cfg, dt = setup_camera all_annotator_types = [ @@ -1241,8 +1262,9 @@ def test_all_annotators_low_resolution_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_all_annotators_non_perfect_square_number_camera(setup_camera): +def test_all_annotators_non_perfect_square_number_camera(setup_camera, device): """Test initialization with all supported annotators.""" sim, camera_cfg, dt = setup_camera all_annotator_types = [ @@ -1340,8 +1362,9 @@ def test_all_annotators_non_perfect_square_number_camera(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_all_annotators_instanceable(setup_camera): +def test_all_annotators_instanceable(setup_camera, device): """Test initialization with all supported annotators on instanceable assets.""" sim, camera_cfg, dt = setup_camera all_annotator_types = [ @@ -1470,8 +1493,9 @@ def test_all_annotators_instanceable(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.isaacsim_ci -def test_throughput(setup_camera): +def test_throughput(setup_camera, device): """Test tiled camera throughput.""" sim, camera_cfg, dt = setup_camera # create camera @@ -1507,8 +1531,9 @@ def test_throughput(setup_camera): del camera +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_output_equal_to_usd_camera_intrinsics(setup_camera): +def test_output_equal_to_usd_camera_intrinsics(setup_camera, device): """ Test that the output of the ray caster camera and the usd camera are the same when both are initialized with the same intrinsic matrix. @@ -1599,8 +1624,9 @@ def test_output_equal_to_usd_camera_intrinsics(setup_camera): del camera_usd +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_sensor_print(setup_camera): +def test_sensor_print(setup_camera, device): """Test sensor print is working correctly.""" sim, camera_cfg, _ = setup_camera # Create sensor @@ -1611,8 +1637,9 @@ def test_sensor_print(setup_camera): print(sensor) +@pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.isaacsim_ci -def test_frame_offset_small_resolution(setup_camera): +def test_frame_offset_small_resolution(setup_camera, device): """Test frame offset issue with small resolution camera.""" sim, camera_cfg, dt = setup_camera # Create sensor @@ -1654,8 +1681,9 @@ def test_frame_offset_small_resolution(setup_camera): assert torch.abs(image_after - image_before).mean() > 0.1 # images of same color should be below 0.01 +@pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.isaacsim_ci -def test_frame_offset_large_resolution(setup_camera): +def test_frame_offset_large_resolution(setup_camera, device): """Test frame offset issue with large resolution camera.""" sim, camera_cfg, dt = setup_camera # Create sensor diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index 9e0085a065d..90bfc557c78 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -132,10 +132,13 @@ def check_mesh_collider_settings(mesh_converter: MeshConverter): assert collision_enabled == exp_collision_enabled, "Collision enabled is not the same!" # -- if collision is enabled, check that collision approximation is correct if exp_collision_enabled: - exp_collision_approximation = mesh_converter.cfg.collision_approximation - mesh_collision_api = UsdPhysics.MeshCollisionAPI(mesh_prim) - collision_approximation = mesh_collision_api.GetApproximationAttr().Get() - assert collision_approximation == exp_collision_approximation, "Collision approximation is not the same!" + if mesh_converter.cfg.mesh_collision_props is not None: + exp_collision_approximation = ( + mesh_converter.cfg.mesh_collision_props.usd_func(mesh_prim).GetApproximationAttr().Get() + ) + mesh_collision_api = UsdPhysics.MeshCollisionAPI(mesh_prim) + collision_approximation = mesh_collision_api.GetApproximationAttr().Get() + assert collision_approximation == exp_collision_approximation, "Collision approximation is not the same!" def test_no_change(assets): @@ -229,7 +232,6 @@ def test_collider_no_approximation(assets): collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="none", collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) @@ -241,9 +243,10 @@ def test_collider_no_approximation(assets): def test_collider_convex_hull(assets): """Convert an OBJ file using convex hull approximation""" collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.ConvexHullPropertiesCfg() mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="convexHull", + mesh_collision_props=mesh_collision_prop, collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) @@ -255,9 +258,10 @@ def test_collider_convex_hull(assets): def test_collider_mesh_simplification(assets): """Convert an OBJ file using mesh simplification approximation""" collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.TriangleMeshSimplificationPropertiesCfg() mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="meshSimplification", + mesh_collision_props=mesh_collision_prop, collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) @@ -269,9 +273,10 @@ def test_collider_mesh_simplification(assets): def test_collider_mesh_bounding_cube(assets): """Convert an OBJ file using bounding cube approximation""" collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.BoundingCubePropertiesCfg() mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="boundingCube", + mesh_collision_props=mesh_collision_prop, collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) @@ -283,9 +288,10 @@ def test_collider_mesh_bounding_cube(assets): def test_collider_mesh_bounding_sphere(assets): """Convert an OBJ file using bounding sphere""" collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=True) + mesh_collision_prop = schemas_cfg.BoundingSpherePropertiesCfg() mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="boundingSphere", + mesh_collision_props=mesh_collision_prop, collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) @@ -297,9 +303,10 @@ def test_collider_mesh_bounding_sphere(assets): def test_collider_mesh_no_collision(assets): """Convert an OBJ file using bounding sphere with collision disabled""" collision_props = schemas_cfg.CollisionPropertiesCfg(collision_enabled=False) + mesh_collision_prop = schemas_cfg.BoundingSpherePropertiesCfg() mesh_config = MeshConverterCfg( asset_path=assets["obj"], - collision_approximation="boundingSphere", + mesh_collision_props=mesh_collision_prop, collision_props=collision_props, ) mesh_converter = MeshConverter(mesh_config) diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 950d3d624ef..2de8b457e32 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -67,10 +67,11 @@ import isaacsim.core.utils.prims as prim_utils import omni.kit import omni.kit.commands +from isaacsim.core.api.materials import PhysicsMaterial +from isaacsim.core.api.materials.preview_surface import PreviewSurface +from isaacsim.core.api.objects import DynamicSphere from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner -from isaacsim.core.materials import PhysicsMaterial, PreviewSurface -from isaacsim.core.objects import DynamicSphere from isaacsim.core.prims import RigidPrim, SingleGeometryPrim, SingleRigidPrim from isaacsim.core.utils.extensions import enable_extension from isaacsim.core.utils.viewports import set_camera_view diff --git a/source/isaaclab_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index ccde51a7166..dac5494087e 100644 --- a/source/isaaclab_assets/config/extension.toml +++ b/source/isaaclab_assets/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.2.2" +version = "0.2.3" # Description title = "Isaac Lab Assets" diff --git a/source/isaaclab_assets/data/Environment/Ball.usd b/source/isaaclab_assets/data/Environment/Ball.usd new file mode 100644 index 00000000000..6ace3cdd292 --- /dev/null +++ b/source/isaaclab_assets/data/Environment/Ball.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9beabaf15c79f5fcd4298b4b776f3a5276f07a245447c3e6c86ade6d310b4bb2 +size 304131 diff --git a/source/isaaclab_assets/data/Environment/Field.usd b/source/isaaclab_assets/data/Environment/Field.usd new file mode 100644 index 00000000000..fc182e03668 --- /dev/null +++ b/source/isaaclab_assets/data/Environment/Field.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94da5316831e0d134de43816e9e82f6b0bbca2d7f65b69252b3f7316b0831924 +size 4061 diff --git a/source/isaaclab_assets/data/Environment/Goal_Blue.usd b/source/isaaclab_assets/data/Environment/Goal_Blue.usd new file mode 100644 index 00000000000..5e7401e06c7 --- /dev/null +++ b/source/isaaclab_assets/data/Environment/Goal_Blue.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cd5986da01dc8fd685331d5885d86696fe135121f8429c8ce0772587cfe5577c +size 44379 diff --git a/source/isaaclab_assets/data/Environment/Goal_Red.usd b/source/isaaclab_assets/data/Environment/Goal_Red.usd new file mode 100644 index 00000000000..52b61c730b1 --- /dev/null +++ b/source/isaaclab_assets/data/Environment/Goal_Red.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9fcf1ddd74ece6891566095dd56b78d490d67c9cc246b13ad3c3c19a33cb2fb7 +size 43964 diff --git a/source/isaaclab_assets/data/Environment/textures/color_121212.hdr b/source/isaaclab_assets/data/Environment/textures/color_121212.hdr new file mode 100644 index 00000000000..ba9d306e3d3 --- /dev/null +++ b/source/isaaclab_assets/data/Environment/textures/color_121212.hdr @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c4c6bc7a125abb414d9336a2b03c698f9db650c7917b69068beaedb569cb2291 +size 109 diff --git a/source/isaaclab_assets/data/Robots/K1/k1.usd b/source/isaaclab_assets/data/Robots/K1/k1.usd new file mode 100644 index 00000000000..3781018860b --- /dev/null +++ b/source/isaaclab_assets/data/Robots/K1/k1.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:02a850f0821606238b3a1c29c0925cda1164c0bee6157f885d83896f24a9f92a +size 13964734 diff --git a/source/isaaclab_assets/data/Robots/K1/rsd455.usd b/source/isaaclab_assets/data/Robots/K1/rsd455.usd new file mode 100644 index 00000000000..b28eab68cdf --- /dev/null +++ b/source/isaaclab_assets/data/Robots/K1/rsd455.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:04dc64fe2641cde2232b14f01423aafd2acb11efb844655e5cbdad6216e3b0a3 +size 4453535 diff --git a/source/isaaclab_assets/data/Robots/T1/rsd455.usd b/source/isaaclab_assets/data/Robots/T1/rsd455.usd new file mode 100644 index 00000000000..b28eab68cdf --- /dev/null +++ b/source/isaaclab_assets/data/Robots/T1/rsd455.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:04dc64fe2641cde2232b14f01423aafd2acb11efb844655e5cbdad6216e3b0a3 +size 4453535 diff --git a/source/isaaclab_assets/data/Robots/T1/t1.usd b/source/isaaclab_assets/data/Robots/T1/t1.usd new file mode 100644 index 00000000000..5a66ffe2c18 --- /dev/null +++ b/source/isaaclab_assets/data/Robots/T1/t1.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f53bb9e085739aded9300898b0b791c8bf37607f84c20c50aa37481de5bd8099 +size 12334965 diff --git a/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms.usd b/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms.usd new file mode 100644 index 00000000000..4a42b25da0a --- /dev/null +++ b/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8cdb124c116f3448ff4b1d9e639b12a87ce115ce8369ace7f4f2fa846e65d866 +size 23281420 diff --git a/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms_hand_1.usd b/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms_hand_1.usd new file mode 100644 index 00000000000..3c77b641670 --- /dev/null +++ b/source/isaaclab_assets/data/Robots/T1/t1_with_7dof_arms_hand_1.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f1048188dce963df77a4dd7a5db43dfdf79f957c20dbb5917248b96d0b634749 +size 56836026 diff --git a/source/isaaclab_assets/data/Robots/nao/.asset_hash b/source/isaaclab_assets/data/Robots/nao/.asset_hash new file mode 100644 index 00000000000..573baeeb1af --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/.asset_hash @@ -0,0 +1 @@ +cde6bd827d43c43b4dd09dc1de187d45 \ No newline at end of file diff --git a/source/isaaclab_assets/data/Robots/nao/config.yaml b/source/isaaclab_assets/data/Robots/nao/config.yaml new file mode 100644 index 00000000000..9e5a00d30ed --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/config.yaml @@ -0,0 +1,23 @@ +asset_path: /home/lscaglioni/IsaacLab-nomadz/nao.urdf +usd_dir: /home/lscaglioni/IsaacLab-nomadz/source/isaaclab_assets/data/Robots/nao +usd_file_name: nao.usd +force_usd_conversion: true +make_instanceable: true +fix_base: false +root_link_name: null +link_density: 0.0 +merge_fixed_joints: true +convert_mimic_joints_to_normal_joints: false +joint_drive: + drive_type: force + target_type: position + gains: + stiffness: 0.0 + damping: 0.0 +collider_type: convex_hull +self_collision: false +replace_cylinders_with_capsules: false +collision_from_visuals: false +## +# Generated by UrdfConverter on 2025-06-08 at 15:33:37. +## diff --git a/source/isaaclab_assets/data/Robots/nao/configuration/materials/textures/textureNAO.png b/source/isaaclab_assets/data/Robots/nao/configuration/materials/textures/textureNAO.png new file mode 100644 index 00000000000..4c4dcfbb1d7 Binary files /dev/null and b/source/isaaclab_assets/data/Robots/nao/configuration/materials/textures/textureNAO.png differ diff --git a/source/isaaclab_assets/data/Robots/nao/configuration/nao_base.usd b/source/isaaclab_assets/data/Robots/nao/configuration/nao_base.usd new file mode 100644 index 00000000000..8f03119a4d2 --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/configuration/nao_base.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ec2d50f67688a40710bfd296ca06703dbceb2b8970014fc885665ef673e5eebe +size 8138629 diff --git a/source/isaaclab_assets/data/Robots/nao/configuration/nao_physics.usd b/source/isaaclab_assets/data/Robots/nao/configuration/nao_physics.usd new file mode 100644 index 00000000000..dbe09b0ee0b --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/configuration/nao_physics.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ed3155ab99b80aef842875a40deb8e65446ffc474ab99e004842d2b43d8290f1 +size 12147 diff --git a/source/isaaclab_assets/data/Robots/nao/configuration/nao_sensor.usd b/source/isaaclab_assets/data/Robots/nao/configuration/nao_sensor.usd new file mode 100644 index 00000000000..731ee90ae1f --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/configuration/nao_sensor.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ca0bda9845c9eb1222cfded63febc29a6a6a7323c3c329168f8d656fe48d26f +size 649 diff --git a/source/isaaclab_assets/data/Robots/nao/nao.usd b/source/isaaclab_assets/data/Robots/nao/nao.usd new file mode 100644 index 00000000000..db48db2474c --- /dev/null +++ b/source/isaaclab_assets/data/Robots/nao/nao.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3acd3f6e1a419b02cdfbe7cf5f3e2f2f160b23cc4b8a81af98cfa5e971627403 +size 1601 diff --git a/source/isaaclab_assets/docs/CHANGELOG.rst b/source/isaaclab_assets/docs/CHANGELOG.rst index 85f70e7e8c3..b6582e77e8a 100644 --- a/source/isaaclab_assets/docs/CHANGELOG.rst +++ b/source/isaaclab_assets/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.2.3 (2025-08-11) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Configuration for G1 robot used for locomanipulation tasks. + 0.2.2 (2025-03-10) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/Booster_K1.py b/source/isaaclab_assets/isaaclab_assets/robots/Booster_K1.py new file mode 100644 index 00000000000..89819c15492 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/Booster_K1.py @@ -0,0 +1,120 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Booster K1 Humanoid robot.""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +import os + + +## +# Configuration +## + +BOOSTER_K1_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path = os.path.expanduser("~/IsaacLab-nomadz/source/isaaclab_assets/data/Robots/K1/k1.usd"), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=None, + max_depenetration_velocity=1.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + + ), + + + + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.34), + joint_pos={ + # Head + "AAHead_Yaw": 0.0, + "Head_Pitch": 0.0, + # Arms + ".*_Shoulder_Pitch": 0.2, + "Left_Shoulder_Roll": -1.35, + "Right_Shoulder_Roll": 1.35, + ".*_Elbow_Pitch": 0.0, + "Left_Elbow_Yaw": -0.5, + "Right_Elbow_Yaw": 0.5, + + # Legs + ".*_Hip_Pitch": -0.20, + ".*_Hip_Roll": 0.0, + ".*_Hip_Yaw": 0.0, + ".*_Knee_Pitch": 0.42, + ".*_Ankle_Pitch": -0.23, + ".*_Ankle_Roll": 0.0, + }, + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_Hip_Pitch", + ".*_Hip_Roll", + ".*_Hip_Yaw", + ".*_Knee_Pitch", + + ], + effort_limit_sim={ + ".*_Hip_Pitch": 45.0, + ".*_Hip_Roll": 30.0, + ".*_Hip_Yaw": 30.0, + ".*_Knee_Pitch": 60.0, + + }, + velocity_limit_sim={ + ".*_Hip_Pitch": 12.5, + ".*_Hip_Roll": 10.9, + ".*_Hip_Yaw": 10.9, + ".*_Knee_Pitch": 11.7, + + }, + stiffness=200.0, + damping=5.0, + armature=0.01, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_Ankle_Pitch", ".*_Ankle_Roll"], + effort_limit_sim={".*_Ankle_Pitch": 24, ".*_Ankle_Roll": 15}, + velocity_limit_sim={".*_Ankle_Pitch": 18.8, ".*_Ankle_Roll": 12.4}, + stiffness=50.0, + damping=1.0, + armature=0.01, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_Shoulder_Pitch", + ".*_Shoulder_Roll", + ".*_Elbow_Pitch", + ".*_Elbow_Yaw", + ], + effort_limit_sim=18.0, + velocity_limit_sim=18.8, + stiffness=40.0, + damping=10.0, + armature=0.01, + ), + }, +) +"""Configuration for the Booster K1 Humanoid robot.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/Booster_T1.py b/source/isaaclab_assets/isaaclab_assets/robots/Booster_T1.py new file mode 100644 index 00000000000..ea406850997 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/Booster_T1.py @@ -0,0 +1,118 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Booster T1 Humanoid robot.""" + +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +import os + + +## +# Configuration +## + +BOOSTER_T1_CFG = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path = os.path.expanduser("~/IsaacLab-nomadz/source/isaaclab_assets/data/Robots/T1/t1.usd"), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=None, + max_depenetration_velocity=1.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + copy_from_source=False, + + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.34), + joint_pos={ + # Head + "AAHead_yaw": 0.0, + "Head_pitch": 0.0, + # Arms + ".*_Shoulder_Pitch": 0.2, + "Left_Shoulder_Roll": -1.35, + "Right_Shoulder_Roll": 1.35, + ".*_Elbow_Pitch": 0.0, + "Left_Elbow_Yaw": -0.5, + "Right_Elbow_Yaw": 0.5, + # Waist + "Waist": 0.0, + # Legs + ".*_Hip_Pitch": -0.20, + ".*_Hip_Roll": 0.0, + ".*_Hip_Yaw": 0.0, + ".*_Knee_Pitch": 0.42, + ".*_Ankle_Pitch": -0.23, + ".*_Ankle_Roll": 0.0, + }, + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_Hip_Pitch", + ".*_Hip_Roll", + ".*_Hip_Yaw", + ".*_Knee_Pitch", + "Waist", + ], + effort_limit_sim={ + ".*_Hip_Pitch": 45.0, + ".*_Hip_Roll": 30.0, + ".*_Hip_Yaw": 30.0, + ".*_Knee_Pitch": 60.0, + "Waist": 30.0, + }, + velocity_limit_sim={ + ".*_Hip_Pitch": 12.5, + ".*_Hip_Roll": 10.9, + ".*_Hip_Yaw": 10.9, + ".*_Knee_Pitch": 11.7, + "Waist": 10.88, + }, + stiffness=200.0, + damping=5.0, + armature=0.01, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_Ankle_Pitch", ".*_Ankle_Roll"], + effort_limit_sim={".*_Ankle_Pitch": 24, ".*_Ankle_Roll": 15}, + velocity_limit_sim={".*_Ankle_Pitch": 18.8, ".*_Ankle_Roll": 12.4}, + stiffness=50.0, + damping=1.0, + armature=0.01, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_Shoulder_Pitch", + ".*_Shoulder_Roll", + ".*_Elbow_Pitch", + ".*_Elbow_Yaw", + ], + effort_limit_sim=18.0, + velocity_limit_sim=18.8, + stiffness=40.0, + damping=10.0, + armature=0.01, + ), + }, +) +"""Configuration for the Booster T1 Humanoid robot.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py index a5996104680..5919b80af65 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py @@ -18,6 +18,7 @@ from .humanoid import * from .humanoid_28 import * from .kinova import * +from .kuka_allegro import * from .pick_and_place import * from .quadcopter import * from .ridgeback_franka import * @@ -26,3 +27,5 @@ from .spot import * from .unitree import * from .universal_robots import * +from .Booster_T1 import * +from .Booster_K1 import * diff --git a/source/isaaclab_assets/isaaclab_assets/robots/agibot.py b/source/isaaclab_assets/isaaclab_assets/robots/agibot.py new file mode 100644 index 00000000000..4acce179687 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/agibot.py @@ -0,0 +1,160 @@ +# Copyright (c) 2022-2025, 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 Agibot A2D humanoid robots. + +The following configurations are available: + +* :obj:`AGIBOT_A2D_CFG`: Agibot A2D robot + + +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +AGIBOT_A2D_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Agibot/A2D/A2D_physics.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Body joints + "joint_lift_body": 0.1995, + "joint_body_pitch": 0.6025, + # Head joints + "joint_head_yaw": 0.0, + "joint_head_pitch": 0.6708, + # Left arm joints + "left_arm_joint1": -1.0817, + "left_arm_joint2": 0.5907, + "left_arm_joint3": 0.3442, + "left_arm_joint4": -1.2819, + "left_arm_joint5": 0.6928, + "left_arm_joint6": 1.4725, + "left_arm_joint7": -0.1599, + # Right arm joints + "right_arm_joint1": 1.0817, + "right_arm_joint2": -0.5907, + "right_arm_joint3": -0.3442, + "right_arm_joint4": 1.2819, + "right_arm_joint5": -0.6928, + "right_arm_joint6": -0.7, + "right_arm_joint7": 0.0, + # Left gripper joints + "left_Right_1_Joint": 0.0, + "left_hand_joint1": 0.994, + "left_Right_0_Joint": 0.0, + "left_Left_0_Joint": 0.0, + "left_Right_Support_Joint": 0.994, + "left_Left_Support_Joint": 0.994, + "left_Right_RevoluteJoint": 0.0, + "left_Left_RevoluteJoint": 0.0, + # Right gripper joints + "right_Right_1_Joint": 0.0, + "right_hand_joint1": 0.994, + "right_Right_0_Joint": 0.0, + "right_Left_0_Joint": 0.0, + "right_Right_Support_Joint": 0.994, + "right_Left_Support_Joint": 0.994, + "right_Right_RevoluteJoint": 0.0, + "right_Left_RevoluteJoint": 0.0, + }, + pos=(-0.6, 0.0, -1.05), # init pos of the articulation for teleop + ), + actuators={ + # Body lift and torso actuators + "body": ImplicitActuatorCfg( + joint_names_expr=["joint_lift_body", "joint_body_pitch"], + effort_limit_sim=10000.0, + velocity_limit_sim=2.61, + stiffness=10000000.0, + damping=200.0, + ), + # Head actuators + "head": ImplicitActuatorCfg( + joint_names_expr=["joint_head_yaw", "joint_head_pitch"], + effort_limit_sim=50.0, + velocity_limit_sim=1.0, + stiffness=80.0, + damping=4.0, + ), + # Left arm actuator + "left_arm": ImplicitActuatorCfg( + joint_names_expr=["left_arm_joint[1-7]"], + effort_limit_sim={ + "left_arm_joint1": 2000.0, + "left_arm_joint[2-7]": 1000.0, + }, + velocity_limit_sim=1.57, + stiffness={"left_arm_joint1": 10000000.0, "left_arm_joint[2-7]": 20000.0}, + damping={"left_arm_joint1": 0.0, "left_arm_joint[2-7]": 0.0}, + ), + # Right arm actuator + "right_arm": ImplicitActuatorCfg( + joint_names_expr=["right_arm_joint[1-7]"], + effort_limit_sim={ + "right_arm_joint1": 2000.0, + "right_arm_joint[2-7]": 1000.0, + }, + velocity_limit_sim=1.57, + stiffness={"right_arm_joint1": 10000000.0, "right_arm_joint[2-7]": 20000.0}, + damping={"right_arm_joint1": 0.0, "right_arm_joint[2-7]": 0.0}, + ), + # "left_Right_2_Joint" is excluded from Articulation. + # "left_hand_joint1" is the driver joint, and "left_Right_1_Joint" is the mimic joint. + # "left_.*_Support_Joint" driver joint can be set optionally, to disable the driver, set stiffness and damping to 0.0 below + "left_gripper": ImplicitActuatorCfg( + joint_names_expr=["left_hand_joint1", "left_.*_Support_Joint"], + effort_limit_sim={"left_hand_joint1": 10.0, "left_.*_Support_Joint": 1.0}, + velocity_limit_sim=2.0, + stiffness={"left_hand_joint1": 20.0, "left_.*_Support_Joint": 2.0}, + damping={"left_hand_joint1": 0.10, "left_.*_Support_Joint": 0.01}, + ), + # set PD to zero for passive joints in close-loop gripper + "left_gripper_passive": ImplicitActuatorCfg( + joint_names_expr=["left_.*_(0|1)_Joint", "left_.*_RevoluteJoint"], + effort_limit_sim=10.0, + velocity_limit_sim=10.0, + stiffness=0.0, + damping=0.0, + ), + # "right_Right_2_Joint" is excluded from Articulation. + # "right_hand_joint1" is the driver joint, and "right_Right_1_Joint" is the mimic joint. + # "right_.*_Support_Joint" driver joint can be set optionally, to disable the driver, set stiffness and damping to 0.0 below + "right_gripper": ImplicitActuatorCfg( + joint_names_expr=["right_hand_joint1", "right_.*_Support_Joint"], + effort_limit_sim={"right_hand_joint1": 100.0, "right_.*_Support_Joint": 100.0}, + velocity_limit_sim=10.0, + stiffness={"right_hand_joint1": 20.0, "right_.*_Support_Joint": 2.0}, + damping={"right_hand_joint1": 0.10, "right_.*_Support_Joint": 0.01}, + ), + # set PD to zero for passive joints in close-loop gripper + "right_gripper_passive": ImplicitActuatorCfg( + joint_names_expr=["right_.*_(0|1)_Joint", "right_.*_RevoluteJoint"], + effort_limit_sim=100.0, + velocity_limit_sim=10.0, + stiffness=0.0, + damping=0.0, + ), + }, + soft_joint_pos_limit_factor=1.0, +) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/galbot.py b/source/isaaclab_assets/isaaclab_assets/robots/galbot.py index d7454372591..cdba75d1b8b 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/galbot.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/galbot.py @@ -30,6 +30,7 @@ disable_gravity=True, max_depenetration_velocity=5.0, ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0), activate_contact_sensors=True, ), init_state=ArticulationCfg.InitialStateCfg( diff --git a/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py b/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py new file mode 100644 index 00000000000..d6c86bb3f15 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/kuka_allegro.py @@ -0,0 +1,114 @@ +# Copyright (c) 2022-2025, 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 Kuka-lbr-iiwa arm robots and Allegro Hand. + +The following configurations are available: + +* :obj:`KUKA_ALLEGRO_CFG`: Kuka Allegro with implicit actuator model. + +Reference: + +* https://www.kuka.com/en-us/products/robotics-systems/industrial-robots/lbr-iiwa +* https://www.wonikrobotics.com/robot-hand + +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +KUKA_ALLEGRO_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/KukaAllegro/kuka.usd", + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=True, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1000.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=32, + solver_velocity_iteration_count=1, + sleep_threshold=0.005, + stabilization_threshold=0.0005, + ), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(drive_type="force"), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + rot=(1.0, 0.0, 0.0, 0.0), + joint_pos={ + "iiwa7_joint_(1|2|7)": 0.0, + "iiwa7_joint_3": 0.7854, + "iiwa7_joint_4": 1.5708, + "iiwa7_joint_(5|6)": -1.5708, + "(index|middle|ring)_joint_0": 0.0, + "(index|middle|ring)_joint_1": 0.3, + "(index|middle|ring)_joint_2": 0.3, + "(index|middle|ring)_joint_3": 0.3, + "thumb_joint_0": 1.5, + "thumb_joint_1": 0.60147215, + "thumb_joint_2": 0.33795027, + "thumb_joint_3": 0.60845138, + }, + ), + actuators={ + "kuka_allegro_actuators": ImplicitActuatorCfg( + joint_names_expr=[ + "iiwa7_joint_(1|2|3|4|5|6|7)", + "index_joint_(0|1|2|3)", + "middle_joint_(0|1|2|3)", + "ring_joint_(0|1|2|3)", + "thumb_joint_(0|1|2|3)", + ], + effort_limit_sim={ + "iiwa7_joint_(1|2|3|4|5|6|7)": 300.0, + "index_joint_(0|1|2|3)": 0.5, + "middle_joint_(0|1|2|3)": 0.5, + "ring_joint_(0|1|2|3)": 0.5, + "thumb_joint_(0|1|2|3)": 0.5, + }, + stiffness={ + "iiwa7_joint_(1|2|3|4)": 300.0, + "iiwa7_joint_5": 100.0, + "iiwa7_joint_6": 50.0, + "iiwa7_joint_7": 25.0, + "index_joint_(0|1|2|3)": 3.0, + "middle_joint_(0|1|2|3)": 3.0, + "ring_joint_(0|1|2|3)": 3.0, + "thumb_joint_(0|1|2|3)": 3.0, + }, + damping={ + "iiwa7_joint_(1|2|3|4)": 45.0, + "iiwa7_joint_5": 20.0, + "iiwa7_joint_6": 15.0, + "iiwa7_joint_7": 15.0, + "index_joint_(0|1|2|3)": 0.1, + "middle_joint_(0|1|2|3)": 0.1, + "ring_joint_(0|1|2|3)": 0.1, + "thumb_joint_(0|1|2|3)": 0.1, + }, + friction={ + "iiwa7_joint_(1|2|3|4|5|6|7)": 1.0, + "index_joint_(0|1|2|3)": 0.01, + "middle_joint_(0|1|2|3)": 0.01, + "ring_joint_(0|1|2|3)": 0.01, + "thumb_joint_(0|1|2|3)": 0.01, + }, + ), + }, + soft_joint_pos_limit_factor=1.0, +) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py index ab963aafff5..4e670b22756 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py @@ -14,6 +14,8 @@ * :obj:`H1_MINIMAL_CFG`: H1 humanoid robot with minimal collision bodies * :obj:`G1_CFG`: G1 humanoid robot * :obj:`G1_MINIMAL_CFG`: G1 humanoid robot with minimal collision bodies +* :obj:`G1_29DOF_CFG`: G1 humanoid robot configured for locomanipulation tasks +* :obj:`G1_INSPIRE_FTP_CFG`: G1 29DOF humanoid robot with Inspire 5-finger hand Reference: https://github.com/unitreerobotics/unitree_ros """ @@ -21,7 +23,7 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR ## # Configuration - Actuators. @@ -381,3 +383,229 @@ This configuration removes most collision meshes to speed up simulation. """ + + +G1_29DOF_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Unitree/G1/g1.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + fix_root_link=False, # Configurable - can be set to True for fixed base + solver_position_iteration_count=8, + solver_velocity_iteration_count=4, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.75), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + ".*_hip_pitch_joint": -0.10, + ".*_knee_joint": 0.30, + ".*_ankle_pitch_joint": -0.20, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": DCMotorCfg( + joint_names_expr=[ + ".*_hip_yaw_joint", + ".*_hip_roll_joint", + ".*_hip_pitch_joint", + ".*_knee_joint", + ], + effort_limit={ + ".*_hip_yaw_joint": 88.0, + ".*_hip_roll_joint": 88.0, + ".*_hip_pitch_joint": 88.0, + ".*_knee_joint": 139.0, + }, + velocity_limit={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 32.0, + ".*_hip_pitch_joint": 32.0, + ".*_knee_joint": 20.0, + }, + stiffness={ + ".*_hip_yaw_joint": 100.0, + ".*_hip_roll_joint": 100.0, + ".*_hip_pitch_joint": 100.0, + ".*_knee_joint": 200.0, + }, + damping={ + ".*_hip_yaw_joint": 2.5, + ".*_hip_roll_joint": 2.5, + ".*_hip_pitch_joint": 2.5, + ".*_knee_joint": 5.0, + }, + armature={ + ".*_hip_.*": 0.03, + ".*_knee_joint": 0.03, + }, + saturation_effort=180.0, + ), + "feet": DCMotorCfg( + joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], + stiffness={ + ".*_ankle_pitch_joint": 20.0, + ".*_ankle_roll_joint": 20.0, + }, + damping={ + ".*_ankle_pitch_joint": 0.2, + ".*_ankle_roll_joint": 0.1, + }, + effort_limit={ + ".*_ankle_pitch_joint": 50.0, + ".*_ankle_roll_joint": 50.0, + }, + velocity_limit={ + ".*_ankle_pitch_joint": 37.0, + ".*_ankle_roll_joint": 37.0, + }, + armature=0.03, + saturation_effort=80.0, + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*_joint", + ], + effort_limit={ + "waist_yaw_joint": 88.0, + "waist_roll_joint": 50.0, + "waist_pitch_joint": 50.0, + }, + velocity_limit={ + "waist_yaw_joint": 32.0, + "waist_roll_joint": 37.0, + "waist_pitch_joint": 37.0, + }, + stiffness={ + "waist_yaw_joint": 5000.0, + "waist_roll_joint": 5000.0, + "waist_pitch_joint": 5000.0, + }, + damping={ + "waist_yaw_joint": 5.0, + "waist_roll_joint": 5.0, + "waist_pitch_joint": 5.0, + }, + armature=0.001, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ], + effort_limit=300, + velocity_limit=100, + stiffness=3000.0, + damping=10.0, + armature={ + ".*_shoulder_.*": 0.001, + ".*_elbow_.*": 0.001, + ".*_wrist_.*_joint": 0.001, + }, + ), + "hands": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_index_.*", + ".*_middle_.*", + ".*_thumb_.*", + ], + effort_limit=300, + velocity_limit=100, + stiffness=20, + damping=2, + armature=0.001, + ), + }, + prim_path="/World/envs/env_.*/Robot", +) +"""Configuration for the Unitree G1 Humanoid robot for locomanipulation tasks. + +This configuration sets up the G1 humanoid robot for locomanipulation tasks, +allowing both locomotion and manipulation capabilities. The robot can be configured +for either fixed base or mobile scenarios by modifying the fix_root_link parameter. + +Key features: +- Configurable base (fixed or mobile) via fix_root_link parameter +- Optimized actuator parameters for locomanipulation tasks +- Enhanced hand and arm configurations for manipulation + +Usage examples: + # For fixed base scenarios (upper body manipulation only) + fixed_base_cfg = G1_29DOF_CFG.copy() + fixed_base_cfg.spawn.articulation_props.fix_root_link = True + + # For mobile scenarios (locomotion + manipulation) + mobile_cfg = G1_29DOF_CFG.copy() + mobile_cfg.spawn.articulation_props.fix_root_link = False +""" + +""" +Configuration for the Unitree G1 Humanoid robot with Inspire 5fingers hand. +The Unitree G1 URDF can be found here: https://github.com/unitreerobotics/unitree_ros/tree/master/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf +The Inspire hand URDF is available at: https://github.com/unitreerobotics/xr_teleoperate/tree/main/assets/inspire_hand +The merging code for the hand and robot can be found here: https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/merge_g1_29dof_and_inspire_hand.ipynb, +Necessary modifications should be made to ensure the correct parent–child relationship. +""" +# Inherit PD settings from G1_29DOF_CFG, with minor adjustments for grasping task +G1_INSPIRE_FTP_CFG = G1_29DOF_CFG.copy() +G1_INSPIRE_FTP_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1_29dof_inspire_hand.usd" +G1_INSPIRE_FTP_CFG.spawn.activate_contact_sensors = True +G1_INSPIRE_FTP_CFG.spawn.rigid_props.disable_gravity = True +G1_INSPIRE_FTP_CFG.spawn.articulation_props.fix_root_link = True +G1_INSPIRE_FTP_CFG.init_state = ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.0), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, +) +# Actuator configuration for arms (stability focused for manipulation) +# Increased damping improves stability of arm movements +G1_INSPIRE_FTP_CFG.actuators["arms"] = ImplicitActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ], + effort_limit=300, + velocity_limit=100, + stiffness=3000.0, + damping=100.0, + armature={ + ".*_shoulder_.*": 0.001, + ".*_elbow_.*": 0.001, + ".*_wrist_.*_joint": 0.001, + }, +) +# Actuator configuration for hands (flexibility focused for grasping) +# Lower stiffness and damping to improve finger flexibility when grasping objects +G1_INSPIRE_FTP_CFG.actuators["hands"] = ImplicitActuatorCfg( + joint_names_expr=[ + ".*_index_.*", + ".*_middle_.*", + ".*_thumb_.*", + ".*_ring_.*", + ".*_pinky_.*", + ], + effort_limit_sim=30.0, + velocity_limit_sim=10.0, + stiffness=10.0, + damping=0.2, + armature=0.001, +) diff --git a/source/isaaclab_assets/setup.py b/source/isaaclab_assets/setup.py index 840cc540ec4..10c6330b9d6 100644 --- a/source/isaaclab_assets/setup.py +++ b/source/isaaclab_assets/setup.py @@ -33,6 +33,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/source/isaaclab_mimic/config/extension.toml b/source/isaaclab_mimic/config/extension.toml index 0382ca89c18..1e2b712b6d1 100644 --- a/source/isaaclab_mimic/config/extension.toml +++ b/source/isaaclab_mimic/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "1.0.14" +version = "1.0.15" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index d25d7aefdeb..a27a3d64e38 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +1.0.15 (2025-09-25) + +Fixed +^^^^^ + +* Fixed a bug in the instruction UI logic that caused incorrect switching between XR and non-XR display modes. The instruction display now properly detects and updates the UI based on the teleoperation device (e.g., handtracking/XR vs. keyboard). + + 1.0.14 (2025-09-08) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py index 5c80d5ddbcd..bc573b58d51 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/__init__.py @@ -16,13 +16,6 @@ from .franka_stack_ik_rel_skillgen_env_cfg import FrankaCubeStackIKRelSkillgenEnvCfg from .franka_stack_ik_rel_visuomotor_cosmos_mimic_env_cfg import FrankaCubeStackIKRelVisuomotorCosmosMimicEnvCfg from .franka_stack_ik_rel_visuomotor_mimic_env_cfg import FrankaCubeStackIKRelVisuomotorMimicEnvCfg -from .galbot_stack_rmp_abs_mimic_env import RmpFlowGalbotCubeStackAbsMimicEnv -from .galbot_stack_rmp_abs_mimic_env_cfg import ( - RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg, - RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg, -) -from .galbot_stack_rmp_rel_mimic_env import RmpFlowGalbotCubeStackRelMimicEnv -from .galbot_stack_rmp_rel_mimic_env_cfg import RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg ## # Inverse Kinematics - Relative Pose Control @@ -104,18 +97,22 @@ gym.register( id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-Rel-Mimic-v0", - entry_point="isaaclab_mimic.envs:RmpFlowGalbotCubeStackRelMimicEnv", + entry_point=f"{__name__}.galbot_stack_rmp_rel_mimic_env:RmpFlowGalbotCubeStackRelMimicEnv", kwargs={ - "env_cfg_entry_point": galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg, + "env_cfg_entry_point": ( + f"{__name__}.galbot_stack_rmp_rel_mimic_env_cfg:RmpFlowGalbotLeftArmGripperCubeStackRelMimicEnvCfg" + ), }, disable_env_checker=True, ) gym.register( id="Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-Rel-Mimic-v0", - entry_point="isaaclab_mimic.envs:RmpFlowGalbotCubeStackRelMimicEnv", + entry_point=f"{__name__}.galbot_stack_rmp_rel_mimic_env:RmpFlowGalbotCubeStackRelMimicEnv", kwargs={ - "env_cfg_entry_point": galbot_stack_rmp_rel_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg, + "env_cfg_entry_point": ( + f"{__name__}.galbot_stack_rmp_rel_mimic_env_cfg:RmpFlowGalbotRightArmSuctionCubeStackRelMimicEnvCfg" + ), }, disable_env_checker=True, ) @@ -126,18 +123,47 @@ gym.register( id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-Abs-Mimic-v0", - entry_point="isaaclab_mimic.envs:RmpFlowGalbotCubeStackAbsMimicEnv", + entry_point=f"{__name__}.galbot_stack_rmp_abs_mimic_env:RmpFlowGalbotCubeStackAbsMimicEnv", kwargs={ - "env_cfg_entry_point": galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg, + "env_cfg_entry_point": ( + f"{__name__}.galbot_stack_rmp_abs_mimic_env_cfg:RmpFlowGalbotLeftArmGripperCubeStackAbsMimicEnvCfg" + ), }, disable_env_checker=True, ) gym.register( id="Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-Abs-Mimic-v0", - entry_point="isaaclab_mimic.envs:RmpFlowGalbotCubeStackAbsMimicEnv", + entry_point=f"{__name__}.galbot_stack_rmp_abs_mimic_env:RmpFlowGalbotCubeStackAbsMimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.galbot_stack_rmp_abs_mimic_env_cfg:RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg" + ), + }, + disable_env_checker=True, +) + +## +# Agibot Left Arm: Place Upright Mug with RmpFlow - Relative Pose Control +## +gym.register( + id="Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-Rel-Mimic-v0", + entry_point=f"{__name__}.pick_place_mimic_env:PickPlaceRelMimicEnv", + kwargs={ + "env_cfg_entry_point": ( + f"{__name__}.agibot_place_upright_mug_mimic_env_cfg:RmpFlowAgibotPlaceUprightMugMimicEnvCfg" + ), + }, + disable_env_checker=True, +) +## +# Agibot Right Arm: Place Toy2Box: RmpFlow - Relative Pose Control +## +gym.register( + id="Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-Rel-Mimic-v0", + entry_point=f"{__name__}.pick_place_mimic_env:PickPlaceRelMimicEnv", kwargs={ - "env_cfg_entry_point": galbot_stack_rmp_abs_mimic_env_cfg.RmpFlowGalbotRightArmSuctionCubeStackAbsMimicEnvCfg, + "env_cfg_entry_point": f"{__name__}.agibot_place_toy2box_mimic_env_cfg:RmpFlowAgibotPlaceToy2BoxMimicEnvCfg", }, disable_env_checker=True, ) diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_toy2box_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_toy2box_mimic_env_cfg.py new file mode 100644 index 00000000000..45e53110ab4 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_toy2box_mimic_env_cfg.py @@ -0,0 +1,84 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.place.config.agibot.place_toy2box_rmp_rel_env_cfg import ( + RmpFlowAgibotPlaceToy2BoxEnvCfg, +) + +OBJECT_A_NAME = "toy_truck" +OBJECT_B_NAME = "box" + + +@configclass +class RmpFlowAgibotPlaceToy2BoxMimicEnvCfg(RmpFlowAgibotPlaceToy2BoxEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Agibot Place Toy2Box env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + + self.datagen_config.name = "demo_src_place_toy2box_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref=OBJECT_A_NAME, + # End of final subtask does not need to be detected + subtask_term_signal="grasp", + # No time offsets for the final subtask + subtask_term_offset_range=(2, 10), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + # selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.01, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=15, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref=OBJECT_B_NAME, + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + # selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.01, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=15, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["agibot"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_upright_mug_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_upright_mug_mimic_env_cfg.py new file mode 100644 index 00000000000..f3154c8f64f --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/agibot_place_upright_mug_mimic_env_cfg.py @@ -0,0 +1,81 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.place.config.agibot.place_upright_mug_rmp_rel_env_cfg import ( + RmpFlowAgibotPlaceUprightMugEnvCfg, +) + + +@configclass +class RmpFlowAgibotPlaceUprightMugMimicEnvCfg(RmpFlowAgibotPlaceUprightMugEnvCfg, MimicEnvCfg): + """ + Isaac Lab Mimic environment config class for Agibot Place Upright Mug env. + """ + + def __post_init__(self): + # post init of parents + super().__post_init__() + + self.datagen_config.name = "demo_src_place_upright_mug_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = True + self.datagen_config.generation_num_trials = 10 + self.datagen_config.generation_select_src_per_subtask = True + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="mug", + # End of final subtask does not need to be detected + subtask_term_signal="grasp", + # No time offsets for the final subtask + subtask_term_offset_range=(15, 30), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + # selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.01, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=5, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="mug", + # End of final subtask does not need to be detected + subtask_term_signal=None, + # No time offsets for the final subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + # selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.01, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=15, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["agibot"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pick_place_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pick_place_mimic_env.py new file mode 100644 index 00000000000..9951c39cf2a --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pick_place_mimic_env.py @@ -0,0 +1,178 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils + +from .franka_stack_ik_abs_mimic_env import FrankaCubeStackIKAbsMimicEnv +from .franka_stack_ik_rel_mimic_env import FrankaCubeStackIKRelMimicEnv + + +class PickPlaceRelMimicEnv(FrankaCubeStackIKRelMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for DiffIK / RmpFlow Relative Pose Control env. + + This MimicEnv is used when all observations are in the robot base frame. + """ + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Gets the pose of each object (including rigid objects and articulated objects) in the robot base frame. + + Args: + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A dictionary that maps object names to object pose matrix in robot base frame (4x4 torch.Tensor) + """ + if env_ids is None: + env_ids = slice(None) + + # Get scene state + scene_state = self.scene.get_state(is_relative=True) + rigid_object_states = scene_state["rigid_object"] + articulation_states = scene_state["articulation"] + + # Get robot root pose + robot_root_pose = articulation_states["robot"]["root_pose"] + root_pos = robot_root_pose[env_ids, :3] + root_quat = robot_root_pose[env_ids, 3:7] + + object_pose_matrix = dict() + + # Process rigid objects + for obj_name, obj_state in rigid_object_states.items(): + pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms( + root_pos, root_quat, obj_state["root_pose"][env_ids, :3], obj_state["root_pose"][env_ids, 3:7] + ) + rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base) + object_pose_matrix[obj_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base) + + # Process articulated objects (except robot) + for art_name, art_state in articulation_states.items(): + if art_name != "robot": # Skip robot + pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms( + root_pos, root_quat, art_state["root_pose"][env_ids, :3], art_state["root_pose"][env_ids, 3:7] + ) + rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base) + object_pose_matrix[art_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base) + + return object_pose_matrix + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask term signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask term signal annotation. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + if env_ids is None: + env_ids = slice(None) + + signals = dict() + + subtask_terms = self.obs_buf["subtask_terms"] + if "grasp" in subtask_terms: + signals["grasp"] = subtask_terms["grasp"][env_ids] + + # Handle multiple grasp signals + for i in range(0, len(self.cfg.subtask_configs)): + grasp_key = f"grasp_{i + 1}" + if grasp_key in subtask_terms: + signals[grasp_key] = subtask_terms[grasp_key][env_ids] + # final subtask signal is not needed + return signals + + +class PickPlaceAbsMimicEnv(FrankaCubeStackIKAbsMimicEnv): + """ + Isaac Lab Mimic environment wrapper class for DiffIK / RmpFlow Absolute Pose Control env. + + This MimicEnv is used when all observations are in the robot base frame. + """ + + def get_object_poses(self, env_ids: Sequence[int] | None = None): + """ + Gets the pose of each object (including rigid objects and articulated objects) in the robot base frame. + + Args: + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A dictionary that maps object names to object pose matrix in robot base frame (4x4 torch.Tensor) + """ + if env_ids is None: + env_ids = slice(None) + + # Get scene state + scene_state = self.scene.get_state(is_relative=True) + rigid_object_states = scene_state["rigid_object"] + articulation_states = scene_state["articulation"] + + # Get robot root pose + robot_root_pose = articulation_states["robot"]["root_pose"] + root_pos = robot_root_pose[env_ids, :3] + root_quat = robot_root_pose[env_ids, 3:7] + + object_pose_matrix = dict() + + # Process rigid objects + for obj_name, obj_state in rigid_object_states.items(): + pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms( + root_pos, root_quat, obj_state["root_pose"][env_ids, :3], obj_state["root_pose"][env_ids, 3:7] + ) + rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base) + object_pose_matrix[obj_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base) + + # Process articulated objects (except robot) + for art_name, art_state in articulation_states.items(): + if art_name != "robot": # Skip robot + pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms( + root_pos, root_quat, art_state["root_pose"][env_ids, :3], art_state["root_pose"][env_ids, 3:7] + ) + rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base) + object_pose_matrix[art_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base) + + return object_pose_matrix + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. The implementation of this method is + required if intending to enable automatic subtask term signal annotation when running the + dataset annotation tool. This method can be kept unimplemented if intending to use manual + subtask term signal annotation. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + if env_ids is None: + env_ids = slice(None) + + signals = dict() + + subtask_terms = self.obs_buf["subtask_terms"] + if "grasp" in subtask_terms: + signals["grasp"] = subtask_terms["grasp"][env_ids] + + # Handle multiple grasp signals + for i in range(0, len(self.cfg.subtask_configs)): + grasp_key = f"grasp_{i + 1}" + if grasp_key in subtask_terms: + signals[grasp_key] = subtask_terms[grasp_key][env_ids] + # final subtask signal is not needed + return signals diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py index c782576c363..7b6e491b6c6 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -8,6 +8,8 @@ import gymnasium as gym from .exhaustpipe_gr1t2_mimic_env_cfg import ExhaustPipeGR1T2MimicEnvCfg +from .locomanipulation_g1_mimic_env import LocomanipulationG1MimicEnv +from .locomanipulation_g1_mimic_env_cfg import LocomanipulationG1MimicEnvCfg from .nutpour_gr1t2_mimic_env_cfg import NutPourGR1T2MimicEnvCfg from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg @@ -44,3 +46,10 @@ kwargs={"env_cfg_entry_point": exhaustpipe_gr1t2_mimic_env_cfg.ExhaustPipeGR1T2MimicEnvCfg}, disable_env_checker=True, ) + +gym.register( + id="Isaac-Locomanipulation-G1-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs.pinocchio_envs:LocomanipulationG1MimicEnv", + kwargs={"env_cfg_entry_point": locomanipulation_g1_mimic_env_cfg.LocomanipulationG1MimicEnvCfg}, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py new file mode 100644 index 00000000000..ad612c61b0a --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py @@ -0,0 +1,129 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class LocomanipulationG1MimicEnv(ManagerBasedRLMimicEnv): + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + eef_pos_name = f"{eef_name}_eef_pos" + eef_quat_name = f"{eef_name}_eef_quat" + + target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_wrist_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. + gripper_action_dict: Dictionary of gripper actions for each end-effector. + action_noise_dict: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + + # target position and rotation + target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"]) + target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"]) + + target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) + target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) + + # gripper actions + left_gripper_action = gripper_action_dict["left"] + right_gripper_action = gripper_action_dict["right"] + + if action_noise_dict is not None: + pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) + pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) + quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) + quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) + + target_left_eef_pos += pos_noise_left + target_right_eef_pos += pos_noise_right + target_left_eef_rot_quat += quat_noise_left + target_right_eef_rot_quat += quat_noise_right + + return torch.cat( + ( + target_left_eef_pos, + target_left_eef_rot_quat, + target_right_eef_pos, + target_right_eef_rot_quat, + left_gripper_action, + right_gripper_action, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + target_poses = {} + + target_left_wrist_position = action[:, 0:3] + target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) + target_poses["left"] = target_pose_left + + target_right_wrist_position = action[:, 7:10] + target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) + target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) + target_poses["right"] = target_pose_right + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + return {"left": actions[:, 14:21], "right": actions[:, 21:]} diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py new file mode 100644 index 00000000000..150831a6ee8 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py @@ -0,0 +1,112 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, +) + + +@configclass +class LocomanipulationG1MimicEnvCfg(LocomanipulationG1EnvCfg, MimicEnvCfg): + + def __post_init__(self): + # Call parent post-init + super().__post_init__() + + # Override datagen config values for demonstration generation + self.datagen_config.name = "demo_src_g1_locomanip_demo_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = False + self.datagen_config.generation_num_trials = 1000 + self.datagen_config.generation_select_src_per_subtask = False + self.datagen_config.generation_select_src_per_arm = False + self.datagen_config.generation_relative = False + self.datagen_config.generation_joint_pos = False + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.num_demo_to_render = 10 + self.datagen_config.num_fail_demo_to_render = 25 + self.datagen_config.seed = 1 + + # Subtask configs for right arm + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="idle_right", + # Randomization range for starting index of the first subtask + first_subtask_start_offset_range=(0, 0), + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=3, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["right"] = subtask_configs + + # Subtask configs for left arm + subtask_configs = [] + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + object_ref="object", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.003, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + self.subtask_configs["left"] = subtask_configs diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py new file mode 100644 index 00000000000..63333b6811e --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with locomanipulation SDG utilities.""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py new file mode 100644 index 00000000000..2d2e656e288 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py @@ -0,0 +1,83 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch +from dataclasses import dataclass + + +@dataclass +class LocomanipulationSDGInputData: + """Data container for in-place manipulation recording state. Used during locomanipulation replay.""" + + left_hand_pose_target: torch.Tensor + """The pose of the left hand in world coordinates.""" + + right_hand_pose_target: torch.Tensor + """The pose of the right hand in world coordinates.""" + + left_hand_joint_positions_target: torch.Tensor + """The left hand joint positions.""" + + right_hand_joint_positions_target: torch.Tensor + """The right hand joint positions.""" + + base_pose: torch.Tensor + """The robot base pose in world coordinates.""" + + object_pose: torch.Tensor + """The target object pose in world coordinates.""" + + fixture_pose: torch.Tensor + """The fixture (ie: table) pose in world coordinates.""" + + +@dataclass +class LocomanipulationSDGOutputData: + """A container for data that is recorded during locomanipulation replay. This is the final output of the pipeline""" + + left_hand_pose_target: torch.Tensor | None = None + """The left hand's target pose.""" + + right_hand_pose_target: torch.Tensor | None = None + """The right hand's target pose.""" + + left_hand_joint_positions_target: torch.Tensor | None = None + """The left hand's target joint positions""" + + right_hand_joint_positions_target: torch.Tensor | None = None + """The right hand's target joint positions""" + + base_velocity_target: torch.Tensor | None = None + """The target velocity of the robot base. This value is provided to the underlying base controller or policy.""" + + start_fixture_pose: torch.Tensor | None = None + """The pose of the start fixture (ie: pick-up table).""" + + end_fixture_pose: torch.Tensor | None = None + """The pose of the end / destination fixture (ie: drop-off table)""" + + object_pose: torch.Tensor | None = None + """The pose of the target object.""" + + base_pose: torch.Tensor | None = None + """The pose of the robot base.""" + + data_generation_state: int | None = None + """The state of the the locomanipulation SDG replay script's state machine.""" + + base_goal_pose: torch.Tensor | None = None + """The goal pose of the robot base (ie: the final destination before dropping off the object)""" + + base_goal_approach_pose: torch.Tensor | None = None + """The goal pose provided to the path planner (this may be offset from the final destination to enable approach.)""" + + base_path: torch.Tensor | None = None + """The robot base path as determined by the path planner.""" + + recording_step: int | None = None + """The current recording step used for upper body replay.""" + + obstacle_fixture_poses: torch.Tensor | None = None + """The pose of all obstacle fixtures in the scene.""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py new file mode 100644 index 00000000000..d73d89b0b06 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with environment wrappers for Locomanipulation SDG.""" + +import gymnasium as gym + +gym.register( + id="Isaac-G1-SteeringWheel-Locomanipulation", + entry_point=f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnvCfg", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py new file mode 100644 index 00000000000..1bd87096bfc --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py @@ -0,0 +1,285 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.envs.common import ViewerCfg +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import CameraCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.datasets import EpisodeData + +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGInputData +from isaaclab_mimic.locomanipulation_sdg.scene_utils import HasPose, SceneBody, SceneFixture + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, + LocomanipulationG1SceneCfg, + ObservationsCfg, + manip_mdp, +) + +from .locomanipulation_sdg_env import LocomanipulationSDGEnv +from .locomanipulation_sdg_env_cfg import LocomanipulationSDGEnvCfg, LocomanipulationSDGRecorderManagerCfg + +NUM_FORKLIFTS = 6 +NUM_BOXES = 12 + + +@configclass +class G1LocomanipulationSDGSceneCfg(LocomanipulationG1SceneCfg): + + packing_table_2 = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable2", + init_state=AssetBaseCfg.InitialStateCfg( + pos=[-2, -3.55, -0.3], + # rot=[0, 0, 0, 1]), + rot=[0.9238795, 0, 0, -0.3826834], + ), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + robot_pov_cam = CameraCfg( + prim_path="/World/envs/env_.*/Robot/torso_link/d435_link/camera", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=8.0, clipping_range=(0.1, 20.0)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.9848078, 0.0, -0.1736482, 0.0), convention="world"), + ) + + +# Add forklifts +for i in range(NUM_FORKLIFTS): + setattr( + G1LocomanipulationSDGSceneCfg, + f"forklift_{i}", + AssetBaseCfg( + prim_path=f"/World/envs/env_.*/Forklift{i}", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Forklift/forklift.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ), + ) + +# Add boxes +for i in range(NUM_BOXES): + setattr( + G1LocomanipulationSDGSceneCfg, + f"box_{i}", + AssetBaseCfg( + prim_path=f"/World/envs/env_.*/Box{i}", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Simple_Warehouse/Props/SM_CardBoxB_01_681.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ), + ) + + +@configclass +class G1LocomanipulationSDGObservationsCfg(ObservationsCfg): + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObservationsCfg.PolicyCfg): + + robot_pov_cam = ObsTerm( + func=manip_mdp.image, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class G1LocomanipulationSDGEnvCfg(LocomanipulationG1EnvCfg, LocomanipulationSDGEnvCfg): + """Configuration for the G1 29DoF environment.""" + + viewer: ViewerCfg = ViewerCfg( + eye=(0.0, 3.0, 1.25), lookat=(0.0, 0.0, 0.5), origin_type="asset_body", asset_name="robot", body_name="pelvis" + ) + + # Scene settings + scene: G1LocomanipulationSDGSceneCfg = G1LocomanipulationSDGSceneCfg( + num_envs=1, env_spacing=2.5, replicate_physics=True + ) + recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() + observations: G1LocomanipulationSDGObservationsCfg = G1LocomanipulationSDGObservationsCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 100.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 6 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + +class G1LocomanipulationSDGEnv(LocomanipulationSDGEnv): + + def __init__(self, cfg: G1LocomanipulationSDGEnvCfg, **kwargs): + super().__init__(cfg) + self.sim.set_camera_view([10.5, 10.5, 10.5], [0.0, 0.0, 0.5]) + self._upper_body_dim = self.action_manager.get_term("upper_body_ik").action_dim + self._waist_dim = 0 # self._env.action_manager.get_term("waist_joint_pos").action_dim + self._lower_body_dim = self.action_manager.get_term("lower_body_joint_pos").action_dim + self._frame_pose_dim = 7 + self._number_of_finger_joints = 7 + + def load_input_data(self, episode_data: EpisodeData, step: int) -> LocomanipulationSDGInputData | None: + dataset_action = episode_data.get_action(step) + dataset_state = episode_data.get_state(step) + + if dataset_action is None: + return None + + if dataset_state is None: + return None + + object_pose = dataset_state["rigid_object"]["object"]["root_pose"] + + data = LocomanipulationSDGInputData( + left_hand_pose_target=dataset_action[0:7], + right_hand_pose_target=dataset_action[7:14], + left_hand_joint_positions_target=dataset_action[14:21], + right_hand_joint_positions_target=dataset_action[21:28], + base_pose=episode_data.get_initial_state()["articulation"]["robot"]["root_pose"], + object_pose=object_pose, + fixture_pose=torch.tensor( + [0.0, 0.55, -0.3, 1.0, 0.0, 0.0, 0.0] + ), # Table pose is not recorded for this env. + ) + + return data + + def build_action_vector( + self, + left_hand_pose_target: torch.Tensor, + right_hand_pose_target: torch.Tensor, + left_hand_joint_positions_target: torch.Tensor, + right_hand_joint_positions_target: torch.Tensor, + base_velocity_target: torch.Tensor, + ): + + action = torch.zeros(self.action_space.shape) + + # Set base height + lower_body_index_offset = self._upper_body_dim + self._waist_dim + action[0, lower_body_index_offset + 3 : lower_body_index_offset + 4] = torch.tensor([0.8]) + + # Left hand pose + assert left_hand_pose_target.shape == ( + self._frame_pose_dim, + ), f"Expected pose shape ({self._frame_pose_dim},), got {left_hand_pose_target.shape}" + action[0, : self._frame_pose_dim] = left_hand_pose_target + + # Right hand pose + assert right_hand_pose_target.shape == ( + self._frame_pose_dim, + ), f"Expected pose shape ({self._frame_pose_dim},), got {right_hand_pose_target.shape}" + action[0, self._frame_pose_dim : 2 * self._frame_pose_dim] = right_hand_pose_target + + # Left hand joint positions + assert left_hand_joint_positions_target.shape == (self._number_of_finger_joints,), ( + f"Expected joint_positions shape ({self._number_of_finger_joints},), got" + f" {left_hand_joint_positions_target.shape}" + ) + action[0, 2 * self._frame_pose_dim : 2 * self._frame_pose_dim + self._number_of_finger_joints] = ( + left_hand_joint_positions_target + ) + + # Right hand joint positions + assert right_hand_joint_positions_target.shape == (self._number_of_finger_joints,), ( + f"Expected joint_positions shape ({self._number_of_finger_joints},), got" + f" {right_hand_joint_positions_target.shape}" + ) + action[ + 0, + 2 * self._frame_pose_dim + + self._number_of_finger_joints : 2 * self._frame_pose_dim + + 2 * self._number_of_finger_joints, + ] = right_hand_joint_positions_target + + # Base velocity + assert base_velocity_target.shape == (3,), f"Expected velocity shape (3,), got {base_velocity_target.shape}" + lower_body_index_offset = self._upper_body_dim + self._waist_dim + action[0, lower_body_index_offset : lower_body_index_offset + 3] = base_velocity_target + + return action + + def get_base(self) -> HasPose: + return SceneBody(self.scene, "robot", "pelvis") + + def get_left_hand(self) -> HasPose: + return SceneBody(self.scene, "robot", "left_wrist_yaw_link") + + def get_right_hand(self) -> HasPose: + return SceneBody(self.scene, "robot", "right_wrist_yaw_link") + + def get_object(self) -> HasPose: + return SceneBody(self.scene, "object", "sm_steeringwheel_a01_01") + + def get_start_fixture(self) -> SceneFixture: + return SceneFixture( + self.scene, + "packing_table", + occupancy_map_boundary=np.array([[-1.45, -0.45], [1.45, -0.45], [1.45, 0.45], [-1.45, 0.45]]), + occupancy_map_resolution=0.05, + ) + + def get_end_fixture(self) -> SceneFixture: + return SceneFixture( + self.scene, + "packing_table_2", + occupancy_map_boundary=np.array([[-1.45, -0.45], [1.45, -0.45], [1.45, 0.45], [-1.45, 0.45]]), + occupancy_map_resolution=0.05, + ) + + def get_obstacle_fixtures(self): + + obstacles = [ + SceneFixture( + self.scene, + f"forklift_{i}", + occupancy_map_boundary=np.array([[-1.0, -1.9], [1.0, -1.9], [1.0, 2.1], [-1.0, 2.1]]), + occupancy_map_resolution=0.05, + ) + for i in range(NUM_FORKLIFTS) + ] + + obstacles += [ + SceneFixture( + self.scene, + f"box_{i}", + occupancy_map_boundary=np.array([[-0.5, -0.5], [0.5, -0.5], [0.5, 0.5], [-0.5, 0.5]]), + occupancy_map_resolution=0.05, + ) + for i in range(NUM_BOXES) + ] + + return obstacles diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py new file mode 100644 index 00000000000..6f9c095dac7 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py @@ -0,0 +1,90 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch + +from isaaclab.envs.manager_based_rl_env import ManagerBasedRLEnv +from isaaclab.managers.recorder_manager import RecorderTerm +from isaaclab.utils.datasets import EpisodeData + +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGInputData, LocomanipulationSDGOutputData +from isaaclab_mimic.locomanipulation_sdg.scene_utils import HasPose, SceneFixture + + +class LocomanipulationSDGOutputDataRecorder(RecorderTerm): + + def record_pre_step(self): + output_data: LocomanipulationSDGOutputData = self._env._locomanipulation_sdg_output_data + + output_data_dict = { + "left_hand_pose_target": output_data.left_hand_pose_target[None, :], + "right_hand_pose_target": output_data.right_hand_pose_target[None, :], + "left_hand_joint_positions_target": output_data.left_hand_joint_positions_target[None, :], + "right_hand_joint_positions_target": output_data.right_hand_joint_positions_target[None, :], + "base_velocity_target": output_data.base_velocity_target[None, :], + "start_fixture_pose": output_data.start_fixture_pose, + "end_fixture_pose": output_data.end_fixture_pose, + "object_pose": output_data.object_pose, + "base_pose": output_data.base_pose, + "task": torch.tensor([[output_data.data_generation_state]]), + "base_goal_pose": output_data.base_goal_pose, + "base_goal_approach_pose": output_data.base_goal_approach_pose, + "base_path": output_data.base_path[None, :], + "recording_step": torch.tensor([[output_data.recording_step]]), + "obstacle_fixture_poses": output_data.obstacle_fixture_poses, + } + + return "locomanipulation_sdg_output_data", output_data_dict + + +class LocomanipulationSDGEnv(ManagerBasedRLEnv): + """An abstract base class that wraps the underlying environment, exposing methods needed for integration with + locomanipulation replay. + + This class defines the core methods needed to integrate an environment with the locomanipulation SDG pipeline for + locomanipulation replay. By implementing these methods for a new environment, the environment can be used with + the locomanipulation SDG replay function. + """ + + def load_input_data(self, episode_data: EpisodeData, step: int) -> LocomanipulationSDGInputData: + raise NotImplementedError + + def build_action_vector( + self, + left_hand_pose_target: torch.Tensor, + right_hand_pose_target: torch.Tensor, + left_hand_joint_positions_target: torch.Tensor, + right_hand_joint_positions_target: torch.Tensor, + base_velocity_target: torch.Tensor, + ): + raise NotImplementedError + + def get_base(self) -> HasPose: + """Get the robot base body.""" + raise NotImplementedError + + def get_left_hand(self) -> HasPose: + """Get the robot left hand body.""" + raise NotImplementedError + + def get_right_hand(self) -> HasPose: + """Get the robot right hand body.""" + raise NotImplementedError + + def get_object(self) -> HasPose: + """Get the target object body.""" + raise NotImplementedError + + def get_start_fixture(self) -> SceneFixture: + """Get the start fixture body.""" + raise NotImplementedError + + def get_end_fixture(self) -> SceneFixture: + """Get the end fixture body.""" + raise NotImplementedError + + def get_obstacle_fixtures(self) -> list[SceneFixture]: + """Get the set of obstacle fixtures.""" + raise NotImplementedError diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py new file mode 100644 index 00000000000..77b82710026 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py @@ -0,0 +1,47 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import isaaclab.envs.mdp as base_mdp +from isaaclab.envs.manager_based_rl_env_cfg import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.managers.recorder_manager import RecorderTerm, RecorderTermCfg +from isaaclab.utils import configclass + +from .locomanipulation_sdg_env import LocomanipulationSDGOutputDataRecorder + + +@configclass +class LocomanipulationSDGOutputDataRecorderCfg(RecorderTermCfg): + """Configuration for the step policy observation recorder term.""" + + class_type: type[RecorderTerm] = LocomanipulationSDGOutputDataRecorder + + +@configclass +class LocomanipulationSDGRecorderManagerCfg(ActionStateRecorderManagerCfg): + record_pre_step_locomanipulation_sdg_output_data = LocomanipulationSDGOutputDataRecorderCfg() + + +@configclass +class LocomanipulationSDGTerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=base_mdp.time_out, time_out=True) + + +@configclass +class LocomanipulationSDGEventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=base_mdp.reset_scene_to_default, mode="reset") + + +@configclass +class LocomanipulationSDGEnvCfg(ManagerBasedRLEnvCfg): + recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() + terminations: LocomanipulationSDGTerminationsCfg = LocomanipulationSDGTerminationsCfg() + events: LocomanipulationSDGEventCfg = LocomanipulationSDGEventCfg() diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py new file mode 100644 index 00000000000..077e6439238 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py @@ -0,0 +1,744 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +import enum +import math +import numpy as np +import os +import tempfile +import torch +import yaml +from dataclasses import dataclass + +import cv2 +import PIL.Image +from PIL import ImageDraw +from pxr import Kind, Sdf, Usd, UsdGeom, UsdShade + + +@dataclass +class Point2d: + x: float + y: float + + +ROS_FREESPACE_THRESH_DEFAULT = 0.196 +ROS_OCCUPIED_THRESH_DEFAULT = 0.65 + +OCCUPANCY_MAP_DEFAULT_Z_MIN = 0.1 +OCCUPANCY_MAP_DEFAULT_Z_MAX = 0.62 +OCCUPANCY_MAP_DEFAULT_CELL_SIZE = 0.05 + + +class OccupancyMapDataValue(enum.IntEnum): + UNKNOWN = 0 + FREESPACE = 1 + OCCUPIED = 2 + + def ros_image_value(self, negate: bool = False) -> int: + + values = [0, 127, 255] + + if negate: + values = values[::-1] + + if self == OccupancyMapDataValue.OCCUPIED: + return values[0] + elif self == OccupancyMapDataValue.UNKNOWN: + return values[1] + else: + return values[2] + + +class OccupancyMapMergeMethod(enum.IntEnum): + UNION = 0 + INTERSECTION = 1 + + +class OccupancyMap: + + ROS_IMAGE_FILENAME = "map.png" + ROS_YAML_FILENAME = "map.yaml" + ROS_YAML_TEMPLATE = """ +image: {image_filename} +resolution: {resolution} +origin: {origin} +negate: {negate} +occupied_thresh: {occupied_thresh} +free_thresh: {free_thresh} +""" + + def __init__(self, data: np.ndarray, resolution: int, origin: tuple[int, int, int]) -> None: + self.data = data + self.resolution = resolution # meters per pixel + self.origin = origin # x, y, yaw. where (x, y) is the bottom-left of image + self._width_pixels = data.shape[1] + self._height_pixels = data.shape[0] + + def freespace_mask(self) -> np.ndarray: + """Get a binary mask representing the freespace of the occupancy map. + + Returns: + np.ndarray: The binary mask representing freespace of the occupancy map. + """ + return self.data == OccupancyMapDataValue.FREESPACE + + def unknown_mask(self) -> np.ndarray: + """Get a binary mask representing the unknown area of the occupancy map. + + Returns: + np.ndarray: The binary mask representing unknown area of the occupancy map. + """ + return self.data == OccupancyMapDataValue.UNKNOWN + + def occupied_mask(self) -> np.ndarray: + """Get a binary mask representing the occupied area of the occupancy map. + + Returns: + np.ndarray: The binary mask representing occupied area of the occupancy map. + """ + return self.data == OccupancyMapDataValue.OCCUPIED + + def ros_image(self, negate: bool = False) -> PIL.Image.Image: + """Get the ROS image for the occupancy map. + + Args: + negate (bool, optional): See "negate" in ROS occupancy map documentation. Defaults to False. + + Returns: + PIL.Image.Image: The ROS image for the occupancy map as a PIL image. + """ + occupied_mask = self.occupied_mask() + ros_image = np.zeros(self.occupied_mask().shape, dtype=np.uint8) + ros_image[occupied_mask] = OccupancyMapDataValue.OCCUPIED.ros_image_value(negate) + ros_image[self.unknown_mask()] = OccupancyMapDataValue.UNKNOWN.ros_image_value(negate) + ros_image[self.freespace_mask()] = OccupancyMapDataValue.FREESPACE.ros_image_value(negate) + ros_image = PIL.Image.fromarray(ros_image) + return ros_image + + def ros_yaml(self, negate: bool = False) -> str: + """Get the ROS occupancy map YAML file content. + + Args: + negate (bool, optional): See "negate" in ROS occupancy map documentation. Defaults to False. + + Returns: + str: The ROS occupancy map YAML file contents. + """ + return self.ROS_YAML_TEMPLATE.format( + image_filename=self.ROS_IMAGE_FILENAME, + resolution=self.resolution, + origin=list(self.origin), + negate=1 if negate else 0, + occupied_thresh=ROS_OCCUPIED_THRESH_DEFAULT, + free_thresh=ROS_FREESPACE_THRESH_DEFAULT, + ) + + def save_ros(self, path: str): + """Save the occupancy map to a folder in ROS format. + + This method saves both the ROS formatted PNG image, as well + as the corresponding YAML file. + + Args: + path (str): The output path to save the occupancy map. + """ + if not os.path.exists(path): + os.makedirs(path) + assert os.path.isdir(path) # safety check + self.ros_image().save(os.path.join(path, self.ROS_IMAGE_FILENAME)) + with open(os.path.join(path, self.ROS_YAML_FILENAME), "w", encoding="utf-8") as f: + f.write(self.ros_yaml()) + + @staticmethod + def from_ros_yaml(ros_yaml_path: str) -> "OccupancyMap": + """Load an occupancy map from a ROS YAML file. + + This method loads an occupancy map from a ROS yaml file. + This method looks up the occupancy map image from the + value specified in the YAML file, and requires that + the image exists at the specified path. + + Args: + ros_yaml_path (str): The path to the ROS yaml file. + + Returns: + _type_: OccupancyMap + """ + with open(ros_yaml_path, encoding="utf-8") as f: + yaml_data = yaml.safe_load(f) + yaml_dir = os.path.dirname(ros_yaml_path) + image_path = os.path.join(yaml_dir, yaml_data["image"]) + image = PIL.Image.open(image_path).convert("L") + occupancy_map = OccupancyMap.from_ros_image( + ros_image=image, + resolution=yaml_data["resolution"], + origin=yaml_data["origin"], + negate=yaml_data["negate"], + occupied_thresh=yaml_data["occupied_thresh"], + free_thresh=yaml_data["free_thresh"], + ) + return occupancy_map + + @staticmethod + def from_ros_image( + ros_image: PIL.Image.Image, + resolution: float, + origin: tuple[float, float, float], + negate: bool = False, + occupied_thresh: float = ROS_OCCUPIED_THRESH_DEFAULT, + free_thresh: float = ROS_FREESPACE_THRESH_DEFAULT, + ) -> "OccupancyMap": + """Create an occupancy map from a ROS formatted image, and other data. + + This method is intended to be used as a utility by other methods, + but not necessarily useful for end use cases. + + Args: + ros_image (PIL.Image.Image): The ROS formatted PIL image. + resolution (float): The resolution (meter/px) of the occupancy map. + origin (tp.Tuple[float, float, float]): The origin of the occupancy map in world coordinates. + negate (bool, optional): See "negate" in ROS occupancy map documentation. Defaults to False. + occupied_thresh (float, optional): The threshold to consider a value occupied. + Defaults to ROS_OCCUPIED_THRESH_DEFAULT. + free_thresh (float, optional): The threshold to consider a value free. Defaults to + ROS_FREESPACE_THRESH_DEFAULT. + + Returns: + OccupancyMap: The occupancy map. + """ + ros_image = ros_image.convert("L") + + free_thresh = free_thresh * 255 + occupied_thresh = occupied_thresh * 255 + + data = np.asarray(ros_image) + + if not negate: + data = 255 - data + + freespace_mask = data < free_thresh + occupied_mask = data > occupied_thresh + + return OccupancyMap.from_masks( + freespace_mask=freespace_mask, occupied_mask=occupied_mask, resolution=resolution, origin=origin + ) + + @staticmethod + def from_masks( + freespace_mask: np.ndarray, occupied_mask: np.ndarray, resolution: float, origin: tuple[float, float, float] + ) -> "OccupancyMap": + """Creates an occupancy map from binary masks and other data + + This method is intended as a utility by other methods, but not necessarily + useful for end use cases. + + Args: + freespace_mask (np.ndarray): Binary mask for the freespace region. + occupied_mask (np.ndarray): Binary mask for the occupied region. + resolution (float): The resolution of the map (meters/px). + origin (tp.Tuple[float, float, float]): The origin of the map in world coordinates. + + Returns: + OccupancyMap: The occupancy map. + """ + + data = np.zeros(freespace_mask.shape, dtype=np.uint8) + data[...] = OccupancyMapDataValue.UNKNOWN + data[freespace_mask] = OccupancyMapDataValue.FREESPACE + data[occupied_mask] = OccupancyMapDataValue.OCCUPIED + + occupancy_map = OccupancyMap(data=data, resolution=resolution, origin=origin) + + return occupancy_map + + @staticmethod + def from_occupancy_boundary(boundary: np.ndarray, resolution: float) -> "OccupancyMap": + min_xy = boundary.min(axis=0) + max_xy = boundary.max(axis=0) + origin = (float(min_xy[0]), float(min_xy[1]), 0.0) + width_meters = max_xy[0] - min_xy[0] + height_meters = max_xy[1] - min_xy[1] + width_pixels = math.ceil(width_meters / resolution) + height_pixels = math.ceil(height_meters / resolution) + + points = boundary + + bot_left_world = (origin[0], origin[1]) + u = (points[:, 0] - bot_left_world[0]) / width_meters + v = 1.0 - (points[:, 1] - bot_left_world[1]) / height_meters + x_px = u * width_pixels + y_px = v * height_pixels + + xy_px = np.concatenate([x_px[:, None], y_px[:, None]], axis=-1).flatten() + + image = np.zeros((height_pixels, width_pixels, 4), dtype=np.uint8) + image = PIL.Image.fromarray(image) + draw = ImageDraw.Draw(image) + draw.polygon(xy_px.tolist(), fill="white", outline="red") + image = np.asarray(image) + + occupied_mask = image[:, :, 0] > 0 + + freespace_mask = ~occupied_mask + + return OccupancyMap.from_masks(freespace_mask, occupied_mask, resolution, origin) + + @staticmethod + def make_empty(start: tuple[float, float], end: tuple[float, float], resolution: float) -> "OccupancyMap": + origin = (start[0], start[1], 0.0) + width_meters = end[0] - start[0] + height_meters = end[1] - start[1] + width_pixels = math.ceil(width_meters / resolution) + height_pixels = math.ceil(height_meters / resolution) + occupied_mask = np.zeros((height_pixels, width_pixels), dtype=np.uint8) > 0 + freespace_mask = np.ones((height_pixels, width_pixels), dtype=np.uint8) > 0 + return OccupancyMap.from_masks(freespace_mask, occupied_mask, resolution, origin) + + def width_pixels(self) -> int: + """Get the width of the occupancy map in pixels. + + Returns: + int: The width in pixels. + """ + return self._width_pixels + + def height_pixels(self) -> int: + """Get the height of the occupancy map in pixels. + + Returns: + int: The height in pixels. + """ + return self._height_pixels + + def width_meters(self) -> float: + """Get the width of the occupancy map in meters. + + Returns: + float: The width in meters. + """ + return self.resolution * self.width_pixels() + + def height_meters(self) -> float: + """Get the height of the occupancy map in meters. + + Returns: + float: The height in meters. + """ + return self.resolution * self.height_pixels() + + def bottom_left_pixel_world_coords(self) -> tuple[float, float]: + """Get the world coordinates of the bottom left pixel. + + Returns: + tp.Tuple[float, float]: The (x, y) world coordinates of the + bottom left pixel in the occupancy map. + """ + return (self.origin[0], self.origin[1]) + + def top_left_pixel_world_coords(self) -> tuple[float, float]: + """Get the world coordinates of the top left pixel. + + Returns: + tp.Tuple[float, float]: The (x, y) world coordinates of the + top left pixel in the occupancy map. + """ + return (self.origin[0], self.origin[1] + self.height_meters()) + + def bottom_right_pixel_world_coords(self) -> tuple[float, float]: + """Get the world coordinates of the bottom right pixel. + + Returns: + tp.Tuple[float, float]: The (x, y) world coordinates of the + bottom right pixel in the occupancy map. + """ + return (self.origin[0] + self.width_meters(), self.origin[1]) + + def top_right_pixel_world_coords(self) -> tuple[float, float]: + """Get the world coordinates of the top right pixel. + + Returns: + tp.Tuple[float, float]: The (x, y) world coordinates of the + top right pixel in the occupancy map. + """ + return (self.origin[0] + self.width_meters(), self.origin[1] + self.height_meters()) + + def buffered(self, buffer_distance_pixels: int) -> "OccupancyMap": + """Get a buffered occupancy map by dilating the occupied regions. + + This method buffers (aka: pads / dilates) an occupancy map by dilating + the occupied regions using a circular mask with the a radius + specified by "buffer_distance_pixels". + + This is useful for modifying an occupancy map for path planning, + collision checking, or robot spawning with the simple assumption + that the robot has a circular collision profile. + + Args: + buffer_distance_pixels (int): The buffer radius / distance in pixels. + + Returns: + OccupancyMap: The buffered (aka: dilated / padded) occupancy map. + """ + + buffer_distance_pixels = int(buffer_distance_pixels) + + radius = buffer_distance_pixels + diameter = radius * 2 + kernel = np.zeros((diameter, diameter), np.uint8) + cv2.circle(kernel, (radius, radius), radius, 255, -1) + occupied = self.occupied_mask().astype(np.uint8) * 255 + occupied_dilated = cv2.dilate(occupied, kernel, iterations=1) + occupied_mask = occupied_dilated == 255 + free_mask = self.freespace_mask() + free_mask[occupied_mask] = False + + return OccupancyMap.from_masks( + freespace_mask=free_mask, occupied_mask=occupied_mask, resolution=self.resolution, origin=self.origin + ) + + def buffered_meters(self, buffer_distance_meters: float) -> "OccupancyMap": + """Get a buffered occupancy map by dilating the occupied regions. + + See OccupancyMap.buffer() for more details. + + Args: + buffer_distance_meters (int): The buffer radius / distance in pixels. + + Returns: + OccupancyMap: The buffered (aka: dilated / padded) occupancy map. + """ + buffer_distance_pixels = int(buffer_distance_meters / self.resolution) + return self.buffered(buffer_distance_pixels) + + def pixel_to_world(self, point: Point2d) -> Point2d: + """Convert a pixel coordinate to world coordinates. + + Args: + point (Point2d): The pixel coordinate. + + Returns: + Point2d: The world coordinate. + """ + # currently doesn't handle rotations + bot_left = self.bottom_left_pixel_world_coords() + u = point.x / self.width_pixels() + v = 1.0 - point.y / self.height_pixels() + x_world = u * self.width_meters() + bot_left[0] + y_world = v * self.height_meters() + bot_left[1] + return Point2d(x=x_world, y=y_world) + + def pixel_to_world_numpy(self, points: np.ndarray) -> np.ndarray: + """Convert an array of pixel coordinates to world coordinates. + + Args: + points (np.ndarray): The Nx2 numpy array of pixel coordinates. + + Returns: + np.ndarray: The Nx2 numpy array of world coordinates. + """ + bot_left = self.bottom_left_pixel_world_coords() + u = points[:, 0] / self.width_pixels() + v = 1.0 - points[:, 1] / self.height_pixels() + x_world = u * self.width_meters() + bot_left[0] + y_world = v * self.height_meters() + bot_left[1] + return np.concatenate([x_world[:, None], y_world[:, None]], axis=-1) + + def world_to_pixel_numpy(self, points: np.ndarray) -> np.ndarray: + """Convert an array of world coordinates to pixel coordinates. + + Args: + points (np.ndarray): The Nx2 numpy array of world coordinates. + + Returns: + np.ndarray: The Nx2 numpy array of pixel coordinates. + """ + bot_left_world = self.bottom_left_pixel_world_coords() + u = (points[:, 0] - bot_left_world[0]) / self.width_meters() + v = 1.0 - (points[:, 1] - bot_left_world[1]) / self.height_meters() + x_px = u * self.width_pixels() + y_px = v * self.height_pixels() + return np.concatenate([x_px[:, None], y_px[:, None]], axis=-1) + + def check_world_point_in_bounds(self, point: Point2d) -> bool: + """Check if a world coordinate is inside the bounds of the occupancy map. + + Args: + point (Point2d): The world coordinate. + + Returns: + bool: True if the coordinate is inside the bounds of + the occupancy map. False otherwise. + """ + + pixel = self.world_to_pixel_numpy(np.array([[point.x, point.y]])) + x_px = int(pixel[0, 0]) + y_px = int(pixel[0, 1]) + + if (x_px < 0) or (x_px >= self.width_pixels()) or (y_px < 0) or (y_px >= self.height_pixels()): + return False + + return True + + def check_world_point_in_freespace(self, point: Point2d) -> bool: + """Check if a world coordinate is inside the freespace region of the occupancy map + + Args: + point (Point2d): The world coordinate. + + Returns: + bool: True if the world coordinate is inside the freespace region of the occupancy map. + False otherwise. + """ + if not self.check_world_point_in_bounds(point): + return False + pixel = self.world_to_pixel_numpy(np.array([[point.x, point.y]])) + x_px = int(pixel[0, 0]) + y_px = int(pixel[0, 1]) + freespace = self.freespace_mask() + return bool(freespace[y_px, x_px]) + + def transformed(self, transform: np.ndarray) -> "OccupancyMap": + return transform_occupancy_map(self, transform) + + def merged(self, other: "OccupancyMap") -> "OccupancyMap": + return merge_occupancy_maps([self, other]) + + +def _omap_world_to_px( + points: np.ndarray, + origin: tuple[float, float, float], + width_meters: float, + height_meters: float, + width_pixels: int, + height_pixels: int, +) -> np.ndarray: + + bot_left_world = (origin[0], origin[1]) + u = (points[:, 0] - bot_left_world[0]) / width_meters + v = 1.0 - (points[:, 1] - bot_left_world[1]) / height_meters + x_px = u * width_pixels + y_px = v * height_pixels + return np.stack([x_px, y_px], axis=-1) + + +def merge_occupancy_maps( + src_omaps: list[OccupancyMap], method: OccupancyMapMergeMethod = OccupancyMapMergeMethod.UNION +) -> OccupancyMap: + """Merge occupancy maps by computing the union or intersection of the occupied regions.""" + dst_resolution = min([o.resolution for o in src_omaps]) + + min_x = min([o.bottom_left_pixel_world_coords()[0] for o in src_omaps]) + min_y = min([o.bottom_left_pixel_world_coords()[1] for o in src_omaps]) + max_x = max([o.top_right_pixel_world_coords()[0] for o in src_omaps]) + max_y = max([o.top_right_pixel_world_coords()[1] for o in src_omaps]) + + dst_origin = (min_x, min_y, 0.0) + + dst_width_meters = max_x - min_x + dst_height_meters = max_y - min_y + dst_width_pixels = math.ceil(dst_width_meters / dst_resolution) + dst_height_pixels = math.ceil(dst_height_meters / dst_resolution) + + dst_occupied_mask: np.ndarray + if method == OccupancyMapMergeMethod.UNION: + dst_occupied_mask = np.zeros((dst_height_pixels, dst_width_pixels), dtype=bool) + elif method == OccupancyMapMergeMethod.INTERSECTION: + dst_occupied_mask = np.ones((dst_height_pixels, dst_width_pixels), dtype=bool) + else: + raise ValueError(f"Unsupported merge method: {method}") + + for src_omap in src_omaps: + + omap_corners_in_world_coords = np.array( + [src_omap.top_left_pixel_world_coords(), src_omap.bottom_right_pixel_world_coords()] + ) + + omap_corners_in_dst_image_coords = ( + _omap_world_to_px( + omap_corners_in_world_coords, + dst_origin, + dst_width_meters, + dst_height_meters, + dst_width_pixels, + dst_height_pixels, + ) + .astype(np.int64) + .flatten() + ) + + omap_dst_width = omap_corners_in_dst_image_coords[2] - omap_corners_in_dst_image_coords[0] + omap_dst_height = omap_corners_in_dst_image_coords[3] - omap_corners_in_dst_image_coords[1] + + omap_occupied_image = PIL.Image.fromarray(255 * src_omap.occupied_mask().astype(np.uint8)).resize( + (omap_dst_width, omap_dst_height) + ) + + omap_occupied_image_tmp = omap_occupied_image.copy() + + dst_occupied_image = PIL.Image.fromarray(np.zeros_like(dst_occupied_mask).astype(np.uint8)) + + dst_occupied_image.paste(omap_occupied_image_tmp, box=omap_corners_in_dst_image_coords) + + if method == OccupancyMapMergeMethod.UNION: + dst_occupied_mask = dst_occupied_mask | (np.asarray(dst_occupied_image) > 0) + elif method == OccupancyMapMergeMethod.INTERSECTION: + dst_occupied_mask = dst_occupied_mask & (np.asarray(dst_occupied_image) > 0) + + dst_occupancy_map = OccupancyMap.from_masks( + freespace_mask=~dst_occupied_mask, occupied_mask=dst_occupied_mask, resolution=dst_resolution, origin=dst_origin + ) + + return dst_occupancy_map + + +def intersect_occupancy_maps(src_omaps: list[OccupancyMap]) -> OccupancyMap: + """Compute a new occupancy map by intersecting the occupied regions of a list of occupancy maps.""" + return merge_occupancy_maps(src_omaps=src_omaps, method=OccupancyMapMergeMethod.INTERSECTION) + + +def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray: + """Transform a set of points by a 2D transform.""" + points = np.concatenate([points, np.ones_like(points[:, 0:1])], axis=-1).T + points = transform @ points + points = points.T + points = points[:, :2] + return points + + +def make_rotate_transform(angle: float) -> np.ndarray: + """Create a 2D rotation transform.""" + return np.array([[np.cos(angle), -np.sin(angle), 0.0], [np.sin(angle), np.cos(angle), 0.0], [0.0, 0.0, 1.0]]) + + +def make_translate_transform(dx: float, dy: float) -> np.ndarray: + """Create a 2D translation transform.""" + return np.array([[1.0, 0.0, dx], [0.0, 1.0, dy], [0.0, 0.0, 1.0]]) + + +def transform_occupancy_map(omap: OccupancyMap, transform: np.ndarray) -> OccupancyMap: + """Transform an occupancy map using a 2D transform.""" + + src_box_world_coords = np.array([ + [omap.origin[0], omap.origin[1]], + [omap.origin[0] + omap.width_meters(), omap.origin[1]], + [omap.origin[0] + omap.width_meters(), omap.origin[1] + omap.height_meters()], + [omap.origin[0], omap.origin[1] + omap.height_meters()], + ]) + + src_box_pixel_coords = omap.world_to_pixel_numpy(src_box_world_coords) + + dst_box_world_coords = transform_points(src_box_world_coords, transform) + + dst_min_xy = np.min(dst_box_world_coords, axis=0) + dst_max_xy = np.max(dst_box_world_coords, axis=0) + + dst_origin = (float(dst_min_xy[0]), float(dst_min_xy[1]), 0) + dst_width_meters = dst_max_xy[0] - dst_min_xy[0] + dst_height_meters = dst_max_xy[1] - dst_min_xy[1] + dst_resolution = omap.resolution + dst_width_pixels = int(dst_width_meters / dst_resolution) + dst_height_pixels = int(dst_height_meters / dst_resolution) + + dst_box_pixel_coords = _omap_world_to_px( + dst_box_world_coords, dst_origin, dst_width_meters, dst_height_meters, dst_width_pixels, dst_height_pixels + ) + + persp_transform = cv2.getPerspectiveTransform( + src_box_pixel_coords.astype(np.float32), dst_box_pixel_coords.astype(np.float32) + ) + + src_occupied_mask = omap.occupied_mask().astype(np.uint8) * 255 + + dst_occupied_mask = cv2.warpPerspective(src_occupied_mask, persp_transform, (dst_width_pixels, dst_height_pixels)) + + dst_occupied_mask = dst_occupied_mask > 0 + dst_freespace_mask = ~dst_occupied_mask + + dst_omap = OccupancyMap.from_masks(dst_freespace_mask, dst_occupied_mask, dst_resolution, dst_origin) + + return dst_omap + + +def occupancy_map_add_to_stage( + occupancy_map: OccupancyMap, + stage: Usd.Stage, + path: str, + z_offset: float = 0.0, + draw_path: np.ndarray | torch.Tensor | None = None, + draw_path_line_width_meter: float = 0.25, +) -> Usd.Prim: + + image_path = os.path.join(tempfile.mkdtemp(), "texture.png") + image = occupancy_map.ros_image() + + if draw_path is not None: + if isinstance(draw_path, torch.Tensor): + draw_path = draw_path.detach().cpu().numpy() + image = image.copy().convert("RGBA") + draw = ImageDraw.Draw(image) + line_coordinates = [] + path_pixels = occupancy_map.world_to_pixel_numpy(draw_path) + for i in range(len(path_pixels)): + line_coordinates.append(int(path_pixels[i, 0])) + line_coordinates.append(int(path_pixels[i, 1])) + width_pixels = draw_path_line_width_meter / occupancy_map.resolution + draw.line(line_coordinates, fill="green", width=int(width_pixels / 2), joint="curve") + + # need to flip, ros uses inverted coordinates on y axis + image = image.transpose(PIL.Image.FLIP_TOP_BOTTOM) + image.save(image_path) + + x0, y0 = occupancy_map.top_left_pixel_world_coords() + x1, y1 = occupancy_map.bottom_right_pixel_world_coords() + + # Add model + modelRoot = UsdGeom.Xform.Define(stage, path) + Usd.ModelAPI(modelRoot).SetKind(Kind.Tokens.component) + + # Add mesh + mesh = UsdGeom.Mesh.Define(stage, os.path.join(path, "mesh")) + mesh.CreatePointsAttr([(x0, y0, z_offset), (x1, y0, z_offset), (x1, y1, z_offset), (x0, y1, z_offset)]) + mesh.CreateFaceVertexCountsAttr([4]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2, 3]) + mesh.CreateExtentAttr([(x0, y0, z_offset), (x1, y1, z_offset)]) + + texCoords = UsdGeom.PrimvarsAPI(mesh).CreatePrimvar( + "st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.varying + ) + + texCoords.Set([(0, 0), (1, 0), (1, 1), (0, 1)]) + + # Add material + material_path = os.path.join(path, "material") + material = UsdShade.Material.Define(stage, material_path) + pbrShader = UsdShade.Shader.Define(stage, os.path.join(material_path, "shader")) + pbrShader.CreateIdAttr("UsdPreviewSurface") + pbrShader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(1.0) + pbrShader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0) + material.CreateSurfaceOutput().ConnectToSource(pbrShader.ConnectableAPI(), "surface") + + # Add texture to material + stReader = UsdShade.Shader.Define(stage, os.path.join(material_path, "st_reader")) + stReader.CreateIdAttr("UsdPrimvarReader_float2") + diffuseTextureSampler = UsdShade.Shader.Define(stage, os.path.join(material_path, "diffuse_texture")) + diffuseTextureSampler.CreateIdAttr("UsdUVTexture") + diffuseTextureSampler.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(image_path) + diffuseTextureSampler.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource( + stReader.ConnectableAPI(), "result" + ) + diffuseTextureSampler.CreateOutput("rgb", Sdf.ValueTypeNames.Float3) + pbrShader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource( + diffuseTextureSampler.ConnectableAPI(), "rgb" + ) + + stInput = material.CreateInput("frame:stPrimvarName", Sdf.ValueTypeNames.Token) + stInput.Set("st") + stReader.CreateInput("varname", Sdf.ValueTypeNames.Token).ConnectToSource(stInput) + mesh.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI) + UsdShade.MaterialBindingAPI(mesh).Bind(material) + + return modelRoot diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py new file mode 100644 index 00000000000..d6a05d34bf4 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py @@ -0,0 +1,215 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +import torch + +from isaacsim.replicator.mobility_gen.impl.path_planner import compress_path, generate_paths + +from .occupancy_map_utils import OccupancyMap +from .scene_utils import HasPose2d + + +def nearest_point_on_segment(a: torch.Tensor, b: torch.Tensor, c: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Find the nearest point on line segment AB to point C. + + This function computes the closest point on the line segment from A to B + to a given point C, along with the distance from A to that point along the segment. + + Args: + a (torch.Tensor): Start point of the line segment. + b (torch.Tensor): End point of the line segment. + c (torch.Tensor): Query point to find the nearest point to. + + Returns: + Tuple[torch.Tensor, torch.Tensor]: A tuple containing: + - The nearest point on the segment AB to point C + - The distance along the segment from A to the nearest point + """ + a2b = b - a + a2c = c - a + a2b_mag = torch.sqrt(torch.sum(a2b**2)) + a2b_norm = a2b / (a2b_mag + 1e-6) + dist = torch.dot(a2c, a2b_norm) + if dist < 0: + return a, dist + elif dist > a2b_mag: + return b, dist + else: + return a + a2b_norm * dist, dist + + +class ParameterizedPath: + """Path parameterized by arc length for distance-based queries and interpolation.""" + + def __init__(self, points: torch.Tensor) -> None: + """Initialize parameterized path with waypoints. + + Args: + points (torch.Tensor): Sequential waypoints of shape (N, 2). + """ + self.points = points + self._init_point_distances() + + def _init_point_distances(self) -> None: + """Initialize arc length parameterization.""" + self._point_distances = torch.zeros(len(self.points)) + length = 0.0 + for i in range(0, len(self.points) - 1): + self._point_distances[i] = length + a = self.points[i] + b = self.points[i + 1] + dist = torch.sqrt(torch.sum((a - b) ** 2)) + length += dist + self._point_distances[-1] = length + + def point_distances(self) -> torch.Tensor: + """Get arc length parameters for each waypoint. + + Returns: + torch.Tensor: Arc length parameter values. + """ + return self._point_distances + + def get_path_length(self) -> float: + """Calculate total path length. + + Returns: + float: Total euclidean distance from start to end. + """ + length = 0.0 + for i in range(1, len(self.points)): + a = self.points[i - 1] + b = self.points[i] + dist = torch.sqrt(torch.sum((a - b) ** 2)) + length += dist + return length + + def points_x(self) -> torch.Tensor: + """Get x-coordinates of all path points. + + Returns: + torch.Tensor: X-coordinates of all points. + """ + return self.points[:, 0] + + def points_y(self) -> torch.Tensor: + """Get y-coordinates of all path points. + + Returns: + torch.Tensor: Y-coordinates of all points. + """ + return self.points[:, 1] + + def get_segment_by_distance(self, distance: float) -> tuple[int, int]: + """Find path segment containing given distance. + + Args: + distance (float): Distance along path from start. + + Returns: + Tuple[int, int]: Indices of segment endpoints. + """ + for i in range(0, len(self.points) - 1): + d_b = self._point_distances[i + 1] + + if distance < d_b: + return (i, i + 1) + + i = len(self.points) - 2 + return (i, i + 1) + + def get_point_by_distance(self, distance: float) -> torch.Tensor: + """Sample point at specified arc length parameter. + + Args: + distance (float): Arc length parameter from start. + + Returns: + torch.Tensor: Interpolated 2D coordinates. + """ + a_idx, b_idx = self.get_segment_by_distance(distance) + a, b = self.points[a_idx], self.points[b_idx] + a_dist, b_dist = self._point_distances[a_idx], self._point_distances[b_idx] + u = (distance - a_dist) / ((b_dist - a_dist) + 1e-6) + u = torch.clip(u, 0.0, 1.0) + return a + u * (b - a) + + def find_nearest(self, point: torch.Tensor) -> tuple[torch.Tensor, float, tuple[int, int], float]: + """Find nearest point on path to query point. + + Args: + point (torch.Tensor): The query point as a 2D tensor. + + Returns: + Tuple containing: + - torch.Tensor: The nearest point on the path to the query point + - float: Distance along the path from the start to the nearest point + - Tuple[int, int]: Indices of the segment containing the nearest point + - float: Euclidean distance from the query point to the nearest point on path + """ + min_pt_dist_to_seg = 1e9 + min_pt_seg = None + min_pt = None + min_pt_dist_along_path = None + + for a_idx in range(0, len(self.points) - 1): + b_idx = a_idx + 1 + a = self.points[a_idx] + b = self.points[b_idx] + nearest_pt, dist_along_seg = nearest_point_on_segment(a, b, point) + dist_to_seg = torch.sqrt(torch.sum((point - nearest_pt) ** 2)) + + if dist_to_seg < min_pt_dist_to_seg: + min_pt_seg = (a_idx, b_idx) + min_pt_dist_to_seg = dist_to_seg + min_pt = nearest_pt + min_pt_dist_along_path = self._point_distances[a_idx] + dist_along_seg + + return min_pt, min_pt_dist_along_path, min_pt_seg, min_pt_dist_to_seg + + +def plan_path(start: HasPose2d, end: HasPose2d, occupancy_map: OccupancyMap) -> torch.Tensor: + """Plan collision-free path between start and end positions. + + Args: + start (HasPose2d): Start entity with 2D pose. + end (HasPose2d): Target entity with 2D pose. + occupancy_map (OccupancyMap): Occupancy map defining obstacles. + + Returns: + torch.Tensor: A tensor of shape (N, 2) representing the planned path as a + sequence of 2D waypoints from start to end. + """ + + # Extract 2D positions from poses + start_world_pos = start.get_pose_2d()[:, :2].numpy() + end_world_pos = end.get_pose_2d()[:, :2].numpy() + + # Convert world coordinates to pixel coordinates + start_xy_pixels = occupancy_map.world_to_pixel_numpy(start_world_pos) + end_xy_pixels = occupancy_map.world_to_pixel_numpy(end_world_pos) + + # Convert from (x, y) to (y, x) format required by path planner + start_yx_pixels = start_xy_pixels[..., 0, ::-1] + end_yx_pixels = end_xy_pixels[..., 0, ::-1] + + # Generate path using the mobility path planner + path_planner_output = generate_paths(start=start_yx_pixels, freespace=occupancy_map.freespace_mask()) + + # Extract and compress the path + path_yx_pixels = path_planner_output.unroll_path(end_yx_pixels) + path_yx_pixels, _ = compress_path(path_yx_pixels) + + # Convert back from (y, x) to (x, y) format + path_xy_pixels = path_yx_pixels[:, ::-1] + + # Convert pixel coordinates back to world coordinates + path_world = occupancy_map.pixel_to_world_numpy(path_xy_pixels) + + # Convert to torch tensor and return + path_tensor = torch.from_numpy(path_world) + + return path_tensor diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py new file mode 100644 index 00000000000..594b6daab0c --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -0,0 +1,190 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import random +import torch + +import isaaclab.utils.math as math_utils + +from .occupancy_map_utils import OccupancyMap, intersect_occupancy_maps +from .transform_utils import transform_mul + + +class HasOccupancyMap: + """An abstract base class for entities that have an associated occupancy map.""" + + def get_occupancy_map(self) -> OccupancyMap: + raise NotImplementedError + + +class HasPose2d: + """An abstract base class for entities that have an associated 2D pose.""" + + def get_pose_2d(self) -> torch.Tensor: + """Get the 2D pose of the entity.""" + raise NotImplementedError + + def get_transform_2d(self): + """Get the 2D transformation matrix of the entity.""" + + pose = self.get_pose_2d() + + x = pose[..., 0] + y = pose[..., 1] + theta = pose[..., 2] + ctheta = torch.cos(theta) + stheta = torch.sin(theta) + + dims = tuple(list(pose.shape)[:-1] + [3, 3]) + transform = torch.zeros(dims) + + transform[..., 0, 0] = ctheta + transform[..., 0, 1] = -stheta + transform[..., 1, 0] = stheta + transform[..., 1, 1] = ctheta + transform[..., 0, 2] = x + transform[..., 1, 2] = y + transform[..., 2, 2] = 1.0 + + return transform + + +class HasPose(HasPose2d): + """An abstract base class for entities that have an associated 3D pose.""" + + def get_pose(self): + """Get the 3D pose of the entity.""" + raise NotImplementedError + + def get_pose_2d(self): + """Get the 2D pose of the entity.""" + pose = self.get_pose() + axis_angle = math_utils.axis_angle_from_quat(pose[..., 3:]) + + yaw = axis_angle[..., 2:3] + xy = pose[..., :2] + + pose_2d = torch.cat([xy, yaw], dim=-1) + + return pose_2d + + +class SceneBody(HasPose): + """A helper class for working with rigid body objects in a scene.""" + + def __init__(self, scene, entity_name: str, body_name: str): + self.scene = scene + self.entity_name = entity_name + self.body_name = body_name + + def get_pose(self): + """Get the 3D pose of the entity.""" + pose = self.scene[self.entity_name].data.body_link_state_w[ + :, + self.scene[self.entity_name].data.body_names.index(self.body_name), + :7, + ] + return pose + + +class SceneAsset(HasPose): + """A helper class for working with assets in a scene.""" + + def __init__(self, scene, entity_name: str): + self.scene = scene + self.entity_name = entity_name + + def get_pose(self): + """Get the 3D pose of the entity.""" + xform_prim = self.scene[self.entity_name] + position, orientation = xform_prim.get_world_poses() + pose = torch.cat([position, orientation], dim=-1) + return pose + + def set_pose(self, pose: torch.Tensor): + """Set the 3D pose of the entity.""" + xform_prim = self.scene[self.entity_name] + position = pose[..., :3] + orientation = pose[..., 3:] + xform_prim.set_world_poses(position, orientation, None) + + +class RelativePose(HasPose): + """A helper class for computing the pose of an entity given it's relative pose to a parent.""" + + def __init__(self, relative_pose: torch.Tensor, parent: HasPose): + self.relative_pose = relative_pose + self.parent = parent + + def get_pose(self): + """Get the 3D pose of the entity.""" + + parent_pose = self.parent.get_pose() + + pose = transform_mul(parent_pose, self.relative_pose) + + return pose + + +class SceneFixture(SceneAsset, HasOccupancyMap): + """A helper class for working with assets in a scene that have an associated occupancy map.""" + + def __init__( + self, scene, entity_name: str, occupancy_map_boundary: np.ndarray, occupancy_map_resolution: float = 0.05 + ): + SceneAsset.__init__(self, scene, entity_name) + self.occupancy_map_boundary = occupancy_map_boundary + self.occupancy_map_resolution = occupancy_map_resolution + + def get_occupancy_map(self): + + local_occupancy_map = OccupancyMap.from_occupancy_boundary( + boundary=self.occupancy_map_boundary, resolution=self.occupancy_map_resolution + ) + + transform = self.get_transform_2d().detach().cpu().numpy() + + occupancy_map = local_occupancy_map.transformed(transform) + + return occupancy_map + + +def place_randomly( + fixture: SceneFixture, background_occupancy_map: OccupancyMap, num_iter: int = 100, area_threshold: float = 1e-5 +): + """Place a scene fixture randomly in an unoccupied region of an occupancy.""" + + # sample random xy in bounds + bottom_left = background_occupancy_map.bottom_left_pixel_world_coords() + top_right = background_occupancy_map.top_right_pixel_world_coords() + + initial_pose = fixture.get_pose() + + for i in range(num_iter): + x = random.uniform(bottom_left[0], top_right[0]) + y = random.uniform(bottom_left[1], top_right[1]) + + yaw = torch.tensor([random.uniform(-torch.pi, torch.pi)]) + roll = torch.zeros_like(yaw) + pitch = torch.zeros_like(yaw) + + quat = math_utils.quat_from_euler_xyz(roll, pitch, yaw) + + new_pose = initial_pose.clone() + new_pose[0, 0] = x + new_pose[0, 1] = y + new_pose[0, 3:] = quat + + fixture.set_pose(new_pose) + + intersection_map = intersect_occupancy_maps([fixture.get_occupancy_map(), background_occupancy_map]) + + intersection_area = np.count_nonzero(intersection_map.occupied_mask()) * (intersection_map.resolution**2) + + if intersection_area < area_threshold: + return True + + return False diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py new file mode 100644 index 00000000000..8f718bebd39 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py @@ -0,0 +1,48 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import torch + +import isaaclab.utils.math as math_utils + + +def transform_mul(transform_a: torch.Tensor, transform_b: torch.Tensor) -> torch.Tensor: + """Multiply two translation, quaternion pose representations by converting to matrices first.""" + # Extract position and quaternion components + pos_a, quat_a = transform_a[..., :3], transform_a[..., 3:] + pos_b, quat_b = transform_b[..., :3], transform_b[..., 3:] + + # Convert quaternions to rotation matrices + rot_a = math_utils.matrix_from_quat(quat_a) + rot_b = math_utils.matrix_from_quat(quat_b) + + # Create pose matrices + pose_a = math_utils.make_pose(pos_a, rot_a) + pose_b = math_utils.make_pose(pos_b, rot_b) + + # Multiply pose matrices + result_pose = torch.matmul(pose_a, pose_b) + + # Extract position and rotation matrix + result_pos, result_rot = math_utils.unmake_pose(result_pose) + + # Convert rotation matrix back to quaternion + result_quat = math_utils.quat_from_matrix(result_rot) + + return torch.cat([result_pos, result_quat], dim=-1) + + +def transform_inv(transform: torch.Tensor) -> torch.Tensor: + """Invert a translation, quaternion format transformation using math_utils.""" + pos, quat = transform[..., :3], transform[..., 3:] + quat_inv = math_utils.quat_inv(quat) + pos_inv = math_utils.quat_apply(quat_inv, -pos) + return torch.cat([pos_inv, quat_inv], dim=-1) + + +def transform_relative_pose(world_pose: torch.Tensor, src_frame_pose: torch.Tensor, dst_frame_pose: torch.Tensor): + """Compute the relative pose with respect to a source frame, and apply this relative pose to a destination frame.""" + pose = transform_mul(dst_frame_pose, transform_mul(transform_inv(src_frame_pose), world_pose)) + return pose diff --git a/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py b/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py index bac7f23eeff..ed2fb3c538e 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py +++ b/source/isaaclab_mimic/isaaclab_mimic/ui/instruction_display.py @@ -20,10 +20,10 @@ class InstructionDisplay: """Handles instruction display for different teleop devices.""" - def __init__(self, teleop_device): - self.teleop_device = teleop_device.lower() + def __init__(self, xr: bool): + self.xr = xr - if "handtracking" in self.teleop_device.lower(): + if self.xr: from isaaclab.ui.xr_widgets import show_instruction self._display_subtask = lambda text: show_instruction( diff --git a/source/isaaclab_mimic/setup.py b/source/isaaclab_mimic/setup.py index 658aed9ee80..95e4c2933f2 100644 --- a/source/isaaclab_mimic/setup.py +++ b/source/isaaclab_mimic/setup.py @@ -57,6 +57,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/source/isaaclab_rl/config/extension.toml b/source/isaaclab_rl/config/extension.toml index 26a2675f922..0e2f31470b6 100644 --- a/source/isaaclab_rl/config/extension.toml +++ b/source/isaaclab_rl/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.3.0" +version = "0.4.4" # Description title = "Isaac Lab RL" diff --git a/source/isaaclab_rl/docs/CHANGELOG.rst b/source/isaaclab_rl/docs/CHANGELOG.rst index d0252ca0dba..e3d44a08d96 100644 --- a/source/isaaclab_rl/docs/CHANGELOG.rst +++ b/source/isaaclab_rl/docs/CHANGELOG.rst @@ -1,6 +1,54 @@ Changelog --------- +0.4.4 (2025-10-15) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Added onnxscript package to isaaclab_rl setup.py to fix onnxscript package missing issue in aarch64 platform. + + +0.4.3 (2025-10-15) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Isaac-Ant-v0's sb3_ppo_cfg default value, so it trains under reasonable amount of time. + + +0.4.2 (2025-10-14) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Updated opset version from 11 to 18 in RSL-RL OnnxPolicyExporter to avoid onnex downcast issue seen in aarch64. + + +0.4.1 (2025-09-09) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Made PBT a bit nicer by +* 1. added resume logic to allow wandb to continue on the same run_id +* 2. corrected broadcasting order in distributed setup +* 3. made score query general by using dotted keys to access dictionary of arbitrary depth + + +0.4.0 (2025-09-09) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Introduced PBT to rl-games. + + 0.3.0 (2025-09-03) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/__init__.py b/source/isaaclab_rl/isaaclab_rl/rl_games/__init__.py new file mode 100644 index 00000000000..38bfa1f4ec3 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Wrappers and utilities to configure an environment for rl-games library.""" + +from .pbt import * +from .rl_games import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/__init__.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py similarity index 70% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/__init__.py rename to source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py index cb907a3f0c8..5eab19288f0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/__init__.py +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/__init__.py @@ -3,6 +3,5 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Locomotion environments for legged robots.""" - -from .tracking import * # noqa +from .pbt import MultiObserver, PbtAlgoObserver +from .pbt_cfg import PbtCfg diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/mutation.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/mutation.py new file mode 100644 index 00000000000..bd6f04be093 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/mutation.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, 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 random +from collections.abc import Callable +from typing import Any + + +def mutate_float(x: float, change_min: float = 1.1, change_max: float = 1.5) -> float: + """Multiply or divide by a random factor in [change_min, change_max].""" + k = random.uniform(change_min, change_max) + return x / k if random.random() < 0.5 else x * k + + +def mutate_discount(x: float, **kwargs) -> float: + """Conservative change near 1.0 by mutating (1 - x) in [1.1, 1.2].""" + inv = 1.0 - x + new_inv = mutate_float(inv, change_min=1.1, change_max=1.2) + return 1.0 - new_inv + + +MUTATION_FUNCS: dict[str, Callable[..., Any]] = { + "mutate_float": mutate_float, + "mutate_discount": mutate_discount, +} + + +def mutate( + params: dict[str, Any], + mutations: dict[str, str], + mutation_rate: float, + change_range: tuple[float, float], +) -> dict[str, Any]: + cmin, cmax = change_range + out: dict[str, Any] = {} + for name, val in params.items(): + fn_name = mutations.get(name) + # skip if no rule or coin flip says "no" + if fn_name is None or random.random() > mutation_rate: + out[name] = val + continue + fn = MUTATION_FUNCS.get(fn_name) + if fn is None: + raise KeyError(f"Unknown mutation function: {fn_name!r}") + out[name] = fn(val, change_min=cmin, change_max=cmax) + return out diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt.py new file mode 100644 index 00000000000..714d5eea183 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt.py @@ -0,0 +1,268 @@ +# Copyright (c) 2022-2025, 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 numpy as np +import os +import random +import sys +import torch +import torch.distributed as dist + +from rl_games.common.algo_observer import AlgoObserver + +from . import pbt_utils +from .mutation import mutate +from .pbt_cfg import PbtCfg + +# i.e. value for target objective when it is not known +_UNINITIALIZED_VALUE = float(-1e9) + + +class PbtAlgoObserver(AlgoObserver): + """rl_games observer that implements Population-Based Training for a single policy process.""" + + def __init__(self, params, args_cli): + """Initialize observer, print the mutation table, and allocate the restart flag. + + Args: + params (dict): Full agent/task params (Hydra style). + args_cli: Parsed CLI args used to reconstruct a restart command. + """ + super().__init__() + self.printer = pbt_utils.PbtTablePrinter() + self.dir = params["pbt"]["directory"] + + self.rendering_args = pbt_utils.RenderingArgs(args_cli) + self.wandb_args = pbt_utils.WandbArgs(args_cli) + self.env_args = pbt_utils.EnvArgs(args_cli) + self.distributed_args = pbt_utils.DistributedArgs(args_cli) + self.cfg = PbtCfg(**params["pbt"]) + self.pbt_it = -1 # dummy value, stands for "not initialized" + self.score = _UNINITIALIZED_VALUE + self.pbt_params = pbt_utils.filter_params(pbt_utils.flatten_dict({"agent": params}), self.cfg.mutation) + + assert len(self.pbt_params) > 0, "[DANGER]: Dictionary that contains params to mutate is empty" + self.printer.print_params_table(self.pbt_params, header="List of params to mutate") + + self.device = params["params"]["config"]["device"] + self.restart_flag = torch.tensor([0], device=self.device) + + def after_init(self, algo): + """Capture training directories on rank 0 and create this policy's workspace folder. + + Args: + algo: rl_games algorithm object (provides writer, train_dir, frame counter, etc.). + """ + if self.distributed_args.rank != 0: + return + + self.algo = algo + self.root_dir = algo.train_dir + self.ws_dir = os.path.join(self.root_dir, self.cfg.workspace) + self.curr_policy_dir = os.path.join(self.ws_dir, f"{self.cfg.policy_idx:03d}") + os.makedirs(self.curr_policy_dir, exist_ok=True) + + def process_infos(self, infos, done_indices): + """Extract the scalar objective from environment infos and store in `self.score`. + + Notes: + Expects the objective to be at `infos[self.cfg.objective]` where self.cfg.objective is dotted address. + """ + score = infos + for part in self.cfg.objective.split("."): + score = score[part] + self.score = score + + def after_steps(self): + """Main PBT tick executed every train step. + + Flow: + 1) Non-zero ranks: exit immediately if `restart_flag == 1`, else return. + 2) Rank 0: if `restart_flag == 1`, restart this process with new params. + 3) Rank 0: on PBT cadence boundary (`interval_steps`), save checkpoint, + load population checkpoints, compute bands, and if this policy is an + underperformer, select a replacement (random leader or self), mutate + whitelisted params, set `restart_flag`, broadcast (if distributed), + and print a mutation diff table. + """ + if self.distributed_args.distributed: + dist.broadcast(self.restart_flag, src=0) + + if self.distributed_args.rank != 0: + if self.restart_flag.cpu().item() == 1: + os._exit(0) + return + + elif self.restart_flag.cpu().item() == 1: + self._restart_with_new_params(self.new_params, self.restart_from_checkpoint) + return + + # Non-zero can continue + if self.distributed_args.rank != 0: + return + + if self.pbt_it == -1: + self.pbt_it = self.algo.frame // self.cfg.interval_steps + return + + if self.algo.frame // self.cfg.interval_steps <= self.pbt_it: + return + + self.pbt_it = self.algo.frame // self.cfg.interval_steps + frame_left = (self.pbt_it + 1) * self.cfg.interval_steps - self.algo.frame + print(f"Policy {self.cfg.policy_idx}, frames_left {frame_left}, PBT it {self.pbt_it}") + try: + pbt_utils.save_pbt_checkpoint(self.curr_policy_dir, self.score, self.pbt_it, self.algo, self.pbt_params) + ckpts = pbt_utils.load_pbt_ckpts(self.ws_dir, self.cfg.policy_idx, self.cfg.num_policies, self.pbt_it) + pbt_utils.cleanup(ckpts, self.curr_policy_dir) + except Exception as exc: + print(f"Policy {self.cfg.policy_idx}: Exception {exc} during sanity log!") + return + + sumry = {i: None if c is None else {k: v for k, v in c.items() if k != "params"} for i, c in ckpts.items()} + self.printer.print_ckpt_summary(sumry) + + policies = list(range(self.cfg.num_policies)) + target_objectives = [ckpts[p]["true_objective"] if ckpts[p] else _UNINITIALIZED_VALUE for p in policies] + initialized = [(obj, p) for obj, p in zip(target_objectives, policies) if obj > _UNINITIALIZED_VALUE] + if not initialized: + print("No policies initialized; skipping PBT iteration.") + return + initialized_objectives, initialized_policies = zip(*initialized) + + # 1) Stats + mean_obj = float(np.mean(initialized_objectives)) + std_obj = float(np.std(initialized_objectives)) + upper_cut = max(mean_obj + self.cfg.threshold_std * std_obj, mean_obj + self.cfg.threshold_abs) + lower_cut = min(mean_obj - self.cfg.threshold_std * std_obj, mean_obj - self.cfg.threshold_abs) + leaders = [p for obj, p in zip(initialized_objectives, initialized_policies) if obj > upper_cut] + underperformers = [p for obj, p in zip(initialized_objectives, initialized_policies) if obj < lower_cut] + + print(f"mean={mean_obj:.4f}, std={std_obj:.4f}, upper={upper_cut:.4f}, lower={lower_cut:.4f}") + print(f"Leaders: {leaders} Underperformers: {underperformers}") + + # 3) Only replace if *this* policy is an underperformer + if self.cfg.policy_idx in underperformers: + # 4) If there are any leaders, pick one at random; else simply mutate with no replacement + replacement_policy_candidate = random.choice(leaders) if leaders else self.cfg.policy_idx + print(f"Replacing policy {self.cfg.policy_idx} with {replacement_policy_candidate}.") + + if self.distributed_args.rank == 0: + for param, value in self.pbt_params.items(): + self.algo.writer.add_scalar(f"pbt/{param}", value, self.algo.frame) + self.algo.writer.add_scalar("pbt/00_best_objective", max(initialized_objectives), self.algo.frame) + self.algo.writer.flush() + + # Decided to replace the policy weights! + cur_params = ckpts[replacement_policy_candidate]["params"] + self.new_params = mutate(cur_params, self.cfg.mutation, self.cfg.mutation_rate, self.cfg.change_range) + self.restart_from_checkpoint = os.path.abspath(ckpts[replacement_policy_candidate]["checkpoint"]) + self.restart_flag[0] = 1 + self.printer.print_mutation_diff(cur_params, self.new_params) + + def _restart_with_new_params(self, new_params, restart_from_checkpoint): + """Re-exec the current process with a filtered/augmented CLI to apply new params. + + Notes: + - Filters out existing Hydra-style overrides that will be replaced, + and appends `--checkpoint=` and new param overrides. + - On distributed runs, assigns a fresh master port and forwards + distributed args to the python.sh launcher. + """ + cli_args = sys.argv + print(f"previous command line args: {cli_args}") + + SKIP = ["checkpoint"] + is_hydra = lambda arg: ( # noqa: E731 + (name := arg.split("=", 1)[0]) not in new_params and not any(k in name for k in SKIP) + ) + modified_args = [cli_args[0]] + [arg for arg in cli_args[1:] if "=" not in arg or is_hydra(arg)] + + modified_args.append(f"--checkpoint={restart_from_checkpoint}") + modified_args.extend(self.wandb_args.get_args_list()) + modified_args.extend(self.rendering_args.get_args_list()) + + # add all of the new (possibly mutated) parameters + for param, value in new_params.items(): + modified_args.append(f"{param}={value}") + + self.algo.writer.flush() + self.algo.writer.close() + + if self.wandb_args.enabled: + import wandb + + # note setdefault will only affect child process, that mean don't have to worry it env variable + # propagate beyond restarted child process + os.environ.setdefault("WANDB_RUN_ID", wandb.run.id) # continue with the same run id + os.environ.setdefault("WANDB_RESUME", "allow") # allow wandb to resume + os.environ.setdefault("WANDB_INIT_TIMEOUT", "300") # give wandb init more time to be fault tolerant + wandb.run.finish() + + # Get the directory of the current file + thisfile_dir = os.path.dirname(os.path.abspath(__file__)) + isaac_sim_path = os.path.abspath(os.path.join(thisfile_dir, "../../../../../_isaac_sim")) + command = [f"{isaac_sim_path}/python.sh"] + + if self.distributed_args.distributed: + self.distributed_args.master_port = str(pbt_utils.find_free_port()) + command.extend(self.distributed_args.get_args_list()) + command += [modified_args[0]] + command.extend(self.env_args.get_args_list()) + command += modified_args[1:] + if self.distributed_args.distributed: + command += ["--distributed"] + + print("Running command:", command, flush=True) + print("sys.executable = ", sys.executable) + print(f"Policy {self.cfg.policy_idx}: Restarting self with args {modified_args}", flush=True) + + if self.distributed_args.rank == 0: + pbt_utils.dump_env_sizes() + + # after any sourcing (or before exec’ing python.sh) prevent kept increasing arg_length: + for var in ("PATH", "PYTHONPATH", "LD_LIBRARY_PATH", "OMNI_USD_RESOLVER_MDL_BUILTIN_PATHS"): + val = os.environ.get(var) + if not val or os.pathsep not in val: + continue + seen = set() + new_parts = [] + for p in val.split(os.pathsep): + if p and p not in seen: + seen.add(p) + new_parts.append(p) + os.environ[var] = os.pathsep.join(new_parts) + + os.execv(f"{isaac_sim_path}/python.sh", command) + + +class MultiObserver(AlgoObserver): + """Meta-observer that allows the user to add several observers.""" + + def __init__(self, observers_): + super().__init__() + self.observers = observers_ + + def _call_multi(self, method, *args_, **kwargs_): + for o in self.observers: + getattr(o, method)(*args_, **kwargs_) + + def before_init(self, base_name, config, experiment_name): + self._call_multi("before_init", base_name, config, experiment_name) + + def after_init(self, algo): + self._call_multi("after_init", algo) + + def process_infos(self, infos, done_indices): + self._call_multi("process_infos", infos, done_indices) + + def after_steps(self): + self._call_multi("after_steps") + + def after_clear_stats(self): + self._call_multi("after_clear_stats") + + def after_print_stats(self, frame, epoch_num, total_time): + self._call_multi("after_print_stats", frame, epoch_num, total_time) diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_cfg.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_cfg.py new file mode 100644 index 00000000000..63cc534edd6 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_cfg.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + + +@configclass +class PbtCfg: + """ + Population-Based Training (PBT) configuration. + + leaders are policies with score > max(mean + threshold_std*std, mean + threshold_abs). + underperformers are policies with score < min(mean - threshold_std*std, mean - threshold_abs). + On replacement, selected hyperparameters are mutated multiplicatively in [change_min, change_max]. + """ + + enabled: bool = False + """Enable/disable PBT logic.""" + + policy_idx: int = 0 + """Index of this learner in the population (unique in [0, num_policies-1]).""" + + num_policies: int = 8 + """Total number of learners participating in PBT.""" + + directory: str = "" + """Root directory for PBT artifacts (checkpoints, metadata).""" + + workspace: str = "pbt_workspace" + """Subfolder under the training dir to isolate this PBT run.""" + + objective: str = "Episode_Reward/success" + """The key in info returned by env.step that pbt measures to determine leaders and underperformers, + If reward is stationary, using the term that corresponds to task success is usually enough, when reward + are non-stationary, consider uses better objectives. + """ + + interval_steps: int = 100_000 + """Environment steps between PBT iterations (save, compare, replace/mutate).""" + + threshold_std: float = 0.10 + """Std-based margin k in max(mean ± k·std, mean ± threshold_abs) for leader/underperformer cuts.""" + + threshold_abs: float = 0.05 + """Absolute margin A in max(mean ± threshold_std·std, mean ± A) for leader/underperformer cuts.""" + + mutation_rate: float = 0.25 + """Per-parameter probability of mutation when a policy is replaced.""" + + change_range: tuple[float, float] = (1.1, 2.0) + """Lower and upper bound of multiplicative change factor (sampled in [change_min, change_max]).""" + + mutation: dict[str, str] = {} + """Mutation strings indicating which parameter will be mutated when pbt restart + example: + { + "agent.params.config.learning_rate": "mutate_float" + "agent.params.config.grad_norm": "mutate_float" + "agent.params.config.entropy_coef": "mutate_float" + } + """ diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_utils.py b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_utils.py new file mode 100644 index 00000000000..2ce88010af5 --- /dev/null +++ b/source/isaaclab_rl/isaaclab_rl/rl_games/pbt/pbt_utils.py @@ -0,0 +1,295 @@ +# Copyright (c) 2022-2025, 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 datetime +import os +import random +import socket +import yaml +from collections import OrderedDict +from pathlib import Path +from prettytable import PrettyTable + +from rl_games.algos_torch.torch_ext import safe_filesystem_op, safe_save + + +class DistributedArgs: + def __init__(self, args_cli): + self.distributed = args_cli.distributed + self.nproc_per_node = int(os.environ.get("WORLD_SIZE", 1)) + self.rank = int(os.environ.get("RANK", 0)) + self.nnodes = 1 + self.master_port = getattr(args_cli, "master_port", None) + + def get_args_list(self) -> list[str]: + args = ["-m", "torch.distributed.run", f"--nnodes={self.nnodes}", f"--nproc_per_node={self.nproc_per_node}"] + if self.master_port: + args.append(f"--master_port={self.master_port}") + return args + + +class EnvArgs: + def __init__(self, args_cli): + self.task = args_cli.task + self.seed = args_cli.seed if args_cli.seed is not None else -1 + self.headless = args_cli.headless + self.num_envs = args_cli.num_envs + + def get_args_list(self) -> list[str]: + list = [] + list.append(f"--task={self.task}") + list.append(f"--seed={self.seed}") + list.append(f"--num_envs={self.num_envs}") + if self.headless: + list.append("--headless") + return list + + +class RenderingArgs: + def __init__(self, args_cli): + self.camera_enabled = args_cli.enable_cameras + self.video = args_cli.video + self.video_length = args_cli.video_length + self.video_interval = args_cli.video_interval + + def get_args_list(self) -> list[str]: + args = [] + if self.camera_enabled: + args.append("--enable_cameras") + if self.video: + args.extend(["--video", f"--video_length={self.video_length}", f"--video_interval={self.video_interval}"]) + return args + + +class WandbArgs: + def __init__(self, args_cli): + self.enabled = args_cli.track + self.project_name = args_cli.wandb_project_name + self.name = args_cli.wandb_name + self.entity = args_cli.wandb_entity + + def get_args_list(self) -> list[str]: + args = [] + if self.enabled: + args.append("--track") + if self.entity: + args.append(f"--wandb-entity={self.entity}") + else: + raise ValueError("entity must be specified if wandb is enabled") + if self.project_name: + args.append(f"--wandb-project-name={self.project_name}") + if self.name: + args.append(f"--wandb-name={self.name}") + return args + + +def dump_env_sizes(): + """Print summary of environment variable usage (count, bytes, top-5 largest, SC_ARG_MAX).""" + + n = len(os.environ) + # total bytes in "KEY=VAL\0" for all envp entries + total = sum(len(k) + 1 + len(v) + 1 for k, v in os.environ.items()) + # find the 5 largest values + biggest = sorted(os.environ.items(), key=lambda kv: len(kv[1]), reverse=True)[:5] + + print(f"[ENV MONITOR] vars={n}, total_bytes={total}") + for k, v in biggest: + print(f" {k!r} length={len(v)} → {v[:60]}{'…' if len(v) > 60 else ''}") + + try: + argmax = os.sysconf("SC_ARG_MAX") + print(f"[ENV MONITOR] SC_ARG_MAX = {argmax}") + except (ValueError, AttributeError): + pass + + +def flatten_dict(d, prefix="", separator="."): + """Flatten nested dictionaries into a flat dict with keys joined by `separator`.""" + + res = dict() + for key, value in d.items(): + if isinstance(value, (dict, OrderedDict)): + res.update(flatten_dict(value, prefix + key + separator, separator)) + else: + res[prefix + key] = value + + return res + + +def find_free_port(max_tries: int = 20) -> int: + """Return an OS-assigned free TCP port, with a few retries; fall back to a random high port.""" + for _ in range(max_tries): + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + try: + s.bind(("", 0)) + return s.getsockname()[1] + except OSError: + continue + return random.randint(20000, 65000) + + +def filter_params(params, params_to_mutate): + """Filter `params` to only those in `params_to_mutate`, converting str floats (e.g. '1e-4') to float.""" + + def try_float(v): + if isinstance(v, str): + try: + return float(v) + except ValueError: + return v + return v + + return {k: try_float(v) for k, v in params.items() if k in params_to_mutate} + + +def save_pbt_checkpoint(workspace_dir, curr_policy_score, curr_iter, algo, params): + """Save a PBT checkpoint (.pth and .yaml) with policy state, score, and metadata (rank 0 only).""" + if int(os.environ.get("RANK", "0")) == 0: + checkpoint_file = os.path.join(workspace_dir, f"{curr_iter:06d}.pth") + safe_save(algo.get_full_state_weights(), checkpoint_file) + pbt_checkpoint_file = os.path.join(workspace_dir, f"{curr_iter:06d}.yaml") + + pbt_checkpoint = { + "iteration": curr_iter, + "true_objective": curr_policy_score, + "frame": algo.frame, + "params": params, + "checkpoint": os.path.abspath(checkpoint_file), + "pbt_checkpoint": os.path.abspath(pbt_checkpoint_file), + "experiment_name": algo.experiment_name, + } + + with open(pbt_checkpoint_file, "w") as fobj: + yaml.dump(pbt_checkpoint, fobj) + + +def load_pbt_ckpts(workspace_dir, cur_policy_id, num_policies, pbt_iteration) -> dict | None: + """ + Load the latest available PBT checkpoint for each policy (≤ current iteration). + Returns a dict mapping policy_idx → checkpoint dict or None. (rank 0 only) + """ + if int(os.environ.get("RANK", "0")) != 0: + return None + checkpoints = dict() + for policy_idx in range(num_policies): + checkpoints[policy_idx] = None + policy_dir = os.path.join(workspace_dir, f"{policy_idx:03d}") + + if not os.path.isdir(policy_dir): + continue + + pbt_checkpoint_files = sorted([f for f in os.listdir(policy_dir) if f.endswith(".yaml")], reverse=True) + for pbt_checkpoint_file in pbt_checkpoint_files: + iteration = int(pbt_checkpoint_file.split(".")[0]) + + # current local time + now_str = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") + ctime_ts = os.path.getctime(os.path.join(policy_dir, pbt_checkpoint_file)) + created_str = datetime.datetime.fromtimestamp(ctime_ts).strftime("%Y-%m-%d %H:%M:%S") + + if iteration <= pbt_iteration: + with open(os.path.join(policy_dir, pbt_checkpoint_file)) as fobj: + now_str = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") + print( + f"Policy {cur_policy_id} [{now_str}]: Loading" + f" policy-{policy_idx} {pbt_checkpoint_file} (created at {created_str})" + ) + checkpoints[policy_idx] = safe_filesystem_op(yaml.load, fobj, Loader=yaml.FullLoader) + break + + return checkpoints + + +def cleanup(checkpoints: dict[int, dict], policy_dir, keep_back: int = 20, max_yaml: int = 50) -> None: + """ + Cleanup old checkpoints for the current policy directory (rank 0 only). + - Delete files older than (oldest iteration - keep_back). + - Keep at most `max_yaml` latest YAML iterations. + """ + if int(os.environ.get("RANK", "0")) == 0: + oldest = min((ckpt["iteration"] if ckpt else 0) for ckpt in checkpoints.values()) + threshold = max(0, oldest - keep_back) + root = Path(policy_dir) + + # group files by numeric iteration (only *.yaml / *.pth) + groups: dict[int, list[Path]] = {} + for p in root.iterdir(): + if p.suffix in (".yaml", ".pth") and p.stem.isdigit(): + groups.setdefault(int(p.stem), []).append(p) + + # 1) drop anything older than threshold + for it in [i for i in groups if i <= threshold]: + for p in groups[it]: + p.unlink(missing_ok=True) + groups.pop(it, None) + + # 2) cap total YAML checkpoints: keep newest `max_yaml` iters + yaml_iters = sorted((i for i, ps in groups.items() if any(p.suffix == ".yaml" for p in ps)), reverse=True) + for it in yaml_iters[max_yaml:]: + for p in groups.get(it, []): + p.unlink(missing_ok=True) + groups.pop(it, None) + + +class PbtTablePrinter: + """All PrettyTable-related rendering lives here.""" + + def __init__(self, *, float_digits: int = 6, path_maxlen: int = 52): + self.float_digits = float_digits + self.path_maxlen = path_maxlen + + # format helpers + def fmt(self, v): + return f"{v:.{self.float_digits}g}" if isinstance(v, float) else v + + def short(self, s: str) -> str: + s = str(s) + L = self.path_maxlen + return s if len(s) <= L else s[: L // 2 - 1] + "…" + s[-L // 2 :] + + # tables + def print_params_table(self, params: dict, header: str = "Parameters"): + table = PrettyTable(field_names=["Parameter", "Value"]) + table.align["Parameter"] = "l" + table.align["Value"] = "r" + for k in sorted(params): + table.add_row([k, self.fmt(params[k])]) + print(header + ":") + print(table.get_string()) + + def print_ckpt_summary(self, sumry: dict[int, dict | None]): + t = PrettyTable(["Policy", "Status", "Objective", "Iter", "Frame", "Experiment", "Checkpoint", "YAML"]) + t.align["Policy"] = "r" + t.align["Status"] = "l" + t.align["Objective"] = "r" + t.align["Iter"] = "r" + t.align["Frame"] = "r" + t.align["Experiment"] = "l" + t.align["Checkpoint"] = "l" + t.align["YAML"] = "l" + for p in sorted(sumry.keys()): + c = sumry[p] + if c is None: + t.add_row([p, "—", "", "", "", "", "", ""]) + else: + t.add_row([ + p, + "OK", + self.fmt(c.get("true_objective", "")), + c.get("iteration", ""), + c.get("frame", ""), + c.get("experiment_name", ""), + self.short(c.get("checkpoint", "")), + self.short(c.get("pbt_checkpoint", "")), + ]) + print(t) + + def print_mutation_diff(self, before: dict, after: dict, *, header: str = "Mutated params (changed only)"): + t = PrettyTable(["Parameter", "Old", "New"]) + for k in sorted(before): + if before[k] != after[k]: + t.add_row([k, self.fmt(before[k]), self.fmt(after[k])]) + print(header + ":") + print(t if t._rows else "(no changes)") diff --git a/source/isaaclab_rl/isaaclab_rl/rl_games.py b/source/isaaclab_rl/isaaclab_rl/rl_games/rl_games.py similarity index 100% rename from source/isaaclab_rl/isaaclab_rl/rl_games.py rename to source/isaaclab_rl/isaaclab_rl/rl_games/rl_games.py diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py index 45cd904ea3c..fc302355741 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/exporter.py @@ -168,6 +168,7 @@ def forward(self, x): def export(self, path, filename): self.to("cpu") self.eval() + opset_version = 18 # was 11, but it caused problems with linux-aarch, and 18 worked well across all systems. if self.is_recurrent: obs = torch.zeros(1, self.rnn.input_size) h_in = torch.zeros(self.rnn.num_layers, 1, self.rnn.hidden_size) @@ -179,7 +180,7 @@ def export(self, path, filename): (obs, h_in, c_in), os.path.join(path, filename), export_params=True, - opset_version=11, + opset_version=opset_version, verbose=self.verbose, input_names=["obs", "h_in", "c_in"], output_names=["actions", "h_out", "c_out"], @@ -191,7 +192,7 @@ def export(self, path, filename): (obs, h_in), os.path.join(path, filename), export_params=True, - opset_version=11, + opset_version=opset_version, verbose=self.verbose, input_names=["obs", "h_in"], output_names=["actions", "h_out"], @@ -206,7 +207,7 @@ def export(self, path, filename): obs, os.path.join(path, filename), export_params=True, - opset_version=11, + opset_version=opset_version, verbose=self.verbose, input_names=["obs"], output_names=["actions"], diff --git a/source/isaaclab_rl/setup.py b/source/isaaclab_rl/setup.py index f9ddcdb0fa5..173c8257c73 100644 --- a/source/isaaclab_rl/setup.py +++ b/source/isaaclab_rl/setup.py @@ -32,7 +32,7 @@ # video recording "moviepy", # make sure this is consistent with isaac sim version - "pillow==11.2.1", + "pillow==11.3.0", "packaging<24", ] @@ -46,7 +46,7 @@ "rl-games @ git+https://github.com/isaac-sim/rl_games.git@python3.11", "gym", ], # rl-games still needs gym :( - "rsl-rl": ["rsl-rl-lib==3.0.1"], + "rsl-rl": ["rsl-rl-lib==3.0.1", "onnxscript>=0.5"], # linux aarch 64 requires manual onnxscript installation } # Add the names with hyphens as aliases for convenience EXTRAS_REQUIRE["rl_games"] = EXTRAS_REQUIRE["rl-games"] @@ -78,6 +78,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index f317365d688..89b8c2c0e0e 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.10.51" +version = "0.11.6" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index ee84acbafd5..e216caab37a 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,69 @@ Changelog --------- +0.11.6 (2025-10-23) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Refined further the anchor position for the XR anchor in the world frame for the G1 robot tasks. + + +0.11.5 (2025-10-22) +~~~~~~~~~~~~~~~~~~~ + +Removed +^^^^^^^ + +* Removed scikit-learn dependency because we are no longer using this package. + + +0.11.4 (2025-10-20) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Fixed the anchor position for the XR anchor in the world frame for the G1 robot tasks. + + +0.11.3 (2025-10-15) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed how the Sim rendering settings are modified by the Cosmos-Mimic env cfg. + + +0.11.2 (2025-10-10) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added OpenXRteleoperation devices to the Galbot stack environments. + + +0.11.1 (2025-09-24) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added dextrous lifting pbt configuration example cfg for rl_games. + + +0.11.0 (2025-09-07) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added dextrous lifting and dextrous reorientation manipulation rl environments. + + 0.10.51 (2025-09-08) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml index 1d0eb42d37c..42917104e36 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml index 9701ac0a8c5..78dcc9de5d1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml index bcaf9abbb5c..693ca6c2b30 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml index 63d05fb1364..f235de692af 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py index 1b869fd2b52..678035f0b0f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py @@ -60,25 +60,16 @@ def __init__(self, cfg: AssemblyEnvCfg, render_mode: str | None = None, **kwargs ) # Create criterion for dynamic time warping (later used for imitation reward) - self.soft_dtw_criterion = SoftDTW(use_cuda=True, gamma=self.cfg_task.soft_dtw_gamma) + cuda_version = automate_algo.get_cuda_version() + if (cuda_version is not None) and (cuda_version < (13, 0, 0)): + self.soft_dtw_criterion = SoftDTW(use_cuda=True, device=self.device, gamma=self.cfg_task.soft_dtw_gamma) + else: + self.soft_dtw_criterion = SoftDTW(use_cuda=False, device=self.device, gamma=self.cfg_task.soft_dtw_gamma) # Evaluate if self.cfg_task.if_logging_eval: self._init_eval_logging() - if self.cfg_task.sample_from != "rand": - self._init_eval_loading() - - def _init_eval_loading(self): - eval_held_asset_pose, eval_fixed_asset_pose, eval_success = automate_log.load_log_from_hdf5( - self.cfg_task.eval_filename - ) - - if self.cfg_task.sample_from == "gp": - self.gp = automate_algo.model_succ_w_gp(eval_held_asset_pose, eval_fixed_asset_pose, eval_success) - elif self.cfg_task.sample_from == "gmm": - self.gmm = automate_algo.model_succ_w_gmm(eval_held_asset_pose, eval_fixed_asset_pose, eval_success) - def _init_eval_logging(self): self.held_asset_pose_log = torch.empty( @@ -246,7 +237,7 @@ def _load_disassembly_data(self): # offset each trajectory to be relative to the goal eef_pos_traj.append(curr_ee_traj - curr_ee_goal) - self.eef_pos_traj = torch.tensor(eef_pos_traj, dtype=torch.float32, device=self.device).squeeze() + self.eef_pos_traj = torch.tensor(np.array(eef_pos_traj), dtype=torch.float32, device=self.device).squeeze() def _get_keypoint_offsets(self, num_keypoints): """Get uniformly-spaced keypoints along a line of unit length, centered at 0.""" @@ -804,28 +795,12 @@ def randomize_held_initial_state(self, env_ids, pre_grasp): torch.rand((self.num_envs,), dtype=torch.float32, device=self.device) ) - if self.cfg_task.sample_from == "rand": - - rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) - held_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] - held_asset_init_pos_rand = torch.tensor( - self.cfg_task.held_asset_init_pos_noise, dtype=torch.float32, device=self.device - ) - self.held_pos_init_rand = held_pos_init_rand @ torch.diag(held_asset_init_pos_rand) - - if self.cfg_task.sample_from == "gp": - rand_sample = torch.rand((self.cfg_task.num_gp_candidates, 3), dtype=torch.float32, device=self.device) - held_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] - held_asset_init_pos_rand = torch.tensor( - self.cfg_task.held_asset_init_pos_noise, dtype=torch.float32, device=self.device - ) - held_asset_init_candidates = held_pos_init_rand @ torch.diag(held_asset_init_pos_rand) - self.held_pos_init_rand, _ = automate_algo.propose_failure_samples_batch_from_gp( - self.gp, held_asset_init_candidates.cpu().detach().numpy(), len(env_ids), self.device - ) - - if self.cfg_task.sample_from == "gmm": - self.held_pos_init_rand = automate_algo.sample_rel_pos_from_gmm(self.gmm, len(env_ids), self.device) + rand_sample = torch.rand((len(env_ids), 3), dtype=torch.float32, device=self.device) + held_pos_init_rand = 2 * (rand_sample - 0.5) # [-1, 1] + held_asset_init_pos_rand = torch.tensor( + self.cfg_task.held_asset_init_pos_noise, dtype=torch.float32, device=self.device + ) + self.held_pos_init_rand = held_pos_init_rand @ torch.diag(held_asset_init_pos_rand) # Set plug pos to assembled state, but offset plug Z-coordinate by height of socket, # minus curriculum displacement diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py index 729402ccc82..f2100216010 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_tasks_cfg.py @@ -139,10 +139,6 @@ class AssemblyTask: num_eval_trials: int = 100 eval_filename: str = "evaluation_00015.h5" - # Fine-tuning - sample_from: str = "rand" # gp, gmm, idv, rand - num_gp_candidates: int = 1000 - @configclass class Peg8mm(HeldAssetCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py index 7edce4a4ddb..86ce3491b16 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/automate_algo_utils.py @@ -3,8 +3,9 @@ # # SPDX-License-Identifier: BSD-3-Clause -import numpy as np import os +import re +import subprocess import sys import torch import trimesh @@ -14,248 +15,60 @@ print("Python Executable:", sys.executable) print("Python Path:", sys.path) -from scipy.stats import norm - -from sklearn.gaussian_process import GaussianProcessRegressor -from sklearn.mixture import GaussianMixture - base_dir = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), ".")) sys.path.append(base_dir) from isaaclab.utils.assets import retrieve_file_path """ -Initialization / Sampling +Util Functions """ -def get_prev_success_init(held_asset_pose, fixed_asset_pose, success, N, device): - """ - Randomly selects N held_asset_pose and corresponding fixed_asset_pose - at indices where success is 1 and returns them as torch tensors. - - Args: - held_asset_pose (np.ndarray): Numpy array of held asset poses. - fixed_asset_pose (np.ndarray): Numpy array of fixed asset poses. - success (np.ndarray): Numpy array of success values (1 for success, 0 for failure). - N (int): Number of successful indices to select. - device: torch device. - - Returns: - tuple: (held_asset_poses, fixed_asset_poses) as torch tensors, or None if no success found. - """ - # Get indices where success is 1 - success_indices = np.where(success == 1)[0] - - if success_indices.size == 0: - return None # No successful entries found - - # Select up to N random indices from successful indices - selected_indices = np.random.choice(success_indices, min(N, len(success_indices)), replace=False) - - return torch.tensor(held_asset_pose[selected_indices], device=device), torch.tensor( - fixed_asset_pose[selected_indices], device=device - ) - - -def model_succ_w_gmm(held_asset_pose, fixed_asset_pose, success): - """ - Models the success rate distribution as a function of the relative position between the held and fixed assets - using a Gaussian Mixture Model (GMM). - - Parameters: - held_asset_pose (np.ndarray): Array of shape (N, 7) representing the positions of the held asset. - fixed_asset_pose (np.ndarray): Array of shape (N, 7) representing the positions of the fixed asset. - success (np.ndarray): Array of shape (N, 1) representing the success. - - Returns: - GaussianMixture: The fitted GMM. - - Example: - gmm = model_succ_dist_w_gmm(held_asset_pose, fixed_asset_pose, success) - relative_pose = held_asset_pose - fixed_asset_pose - # To compute the probability of each component for the given relative positions: - probabilities = gmm.predict_proba(relative_pose) - """ - # Compute the relative positions (held asset relative to fixed asset) - relative_pos = held_asset_pose[:, :3] - fixed_asset_pose[:, :3] - - # Flatten the success array to serve as sample weights. - # This way, samples with higher success contribute more to the model. - sample_weights = success.flatten() - - # Initialize the Gaussian Mixture Model with the specified number of components. - gmm = GaussianMixture(n_components=2, random_state=0) - - # Fit the GMM on the relative positions, using sample weights from the success metric. - gmm.fit(relative_pos, sample_weight=sample_weights) - - return gmm - - -def sample_rel_pos_from_gmm(gmm, batch_size, device): +def parse_cuda_version(version_string): """ - Samples a batch of relative poses (held_asset relative to fixed_asset) - from a fitted GaussianMixture model. + Parse CUDA version string into comparable tuple of (major, minor, patch). - Parameters: - gmm (GaussianMixture): A GaussianMixture model fitted on relative pose data. - batch_size (int): The number of samples to generate. + Args: + version_string: Version string like "12.8.9" or "11.2" - Returns: - torch.Tensor: A tensor of shape (batch_size, 3) containing the sampled relative poses. - """ - # Sample batch_size samples from the Gaussian Mixture Model. - samples, _ = gmm.sample(batch_size) + Returns: + Tuple of (major, minor, patch) as integers, where patch defaults to 0 iff + not present. - # Convert the numpy array to a torch tensor. - samples_tensor = torch.from_numpy(samples).to(device) - - return samples_tensor - - -def model_succ_w_gp(held_asset_pose, fixed_asset_pose, success): + Example: + "12.8.9" -> (12, 8, 9) + "11.2" -> (11, 2, 0) """ - Models the success rate distribution given the relative position of the held asset - from the fixed asset using a Gaussian Process classifier. - - Parameters: - held_asset_pose (np.ndarray): Array of shape (N, 7) representing the held asset pose. - Assumes the first 3 columns are the (x, y, z) positions. - fixed_asset_pose (np.ndarray): Array of shape (N, 7) representing the fixed asset pose. - Assumes the first 3 columns are the (x, y, z) positions. - success (np.ndarray): Array of shape (N, 1) representing the success outcome (e.g., 0 for failure, - 1 for success). - - Returns: - GaussianProcessClassifier: A trained GP classifier that models the success rate. - """ - # Compute the relative position (using only the translation components) - relative_position = held_asset_pose[:, :3] - fixed_asset_pose[:, :3] - - # Flatten success array from (N, 1) to (N,) - y = success.ravel() - - # Create and fit the Gaussian Process Classifier - # gp = GaussianProcessClassifier(kernel=kernel, random_state=42) - gp = GaussianProcessRegressor(random_state=42) - gp.fit(relative_position, y) - - return gp - - -def propose_failure_samples_batch_from_gp( - gp_model, candidate_points, batch_size, device, method="ucb", kappa=2.0, xi=0.01 -): - """ - Proposes a batch of candidate samples from failure-prone regions using one of three acquisition functions: - 'ucb' (Upper Confidence Bound), 'pi' (Probability of Improvement), or 'ei' (Expected Improvement). - - In this formulation, lower predicted success probability (closer to 0) is desired, - so we invert the typical acquisition formulations. - - Parameters: - gp_model: A trained Gaussian Process model (e.g., GaussianProcessRegressor) that supports - predictions with uncertainties via the 'predict' method (with return_std=True). - candidate_points (np.ndarray): Array of shape (n_candidates, d) representing candidate relative positions. - batch_size (int): Number of candidate samples to propose. - method (str): Acquisition function to use: 'ucb', 'pi', or 'ei'. Default is 'ucb'. - kappa (float): Exploration parameter for UCB. Default is 2.0. - xi (float): Exploration parameter for PI and EI. Default is 0.01. - - Returns: - best_candidates (np.ndarray): Array of shape (batch_size, d) containing the selected candidate points. - acquisition (np.ndarray): Acquisition values computed for each candidate point. - """ - # Obtain the predictive mean and standard deviation for each candidate point. - mu, sigma = gp_model.predict(candidate_points, return_std=True) - # mu, sigma = gp_model.predict(candidate_points) - - # Compute the acquisition values based on the chosen method. - if method.lower() == "ucb": - # Inversion: we want low success (i.e. low mu) and high uncertainty (sigma) to be attractive. - acquisition = kappa * sigma - mu - elif method.lower() == "pi": - # Probability of Improvement: likelihood of the prediction falling below the target=0.0. - Z = (-mu - xi) / (sigma + 1e-9) - acquisition = norm.cdf(Z) - elif method.lower() == "ei": - # Expected Improvement - Z = (-mu - xi) / (sigma + 1e-9) - acquisition = (-mu - xi) * norm.cdf(Z) + sigma * norm.pdf(Z) - # Set acquisition to 0 where sigma is nearly zero. - acquisition[sigma < 1e-9] = 0.0 - else: - raise ValueError("Unknown acquisition method. Please choose 'ucb', 'pi', or 'ei'.") - - # Select the indices of the top batch_size candidates (highest acquisition values). - sorted_indices = np.argsort(acquisition)[::-1] # sort in descending order - best_indices = sorted_indices[:batch_size] - best_candidates = candidate_points[best_indices] - - # Convert the numpy array to a torch tensor. - best_candidates_tensor = torch.from_numpy(best_candidates).to(device) - - return best_candidates_tensor, acquisition - - -def propose_success_samples_batch_from_gp( - gp_model, candidate_points, batch_size, device, method="ucb", kappa=2.0, xi=0.01 -): - """ - Proposes a batch of candidate samples from high success rate regions using one of three acquisition functions: - 'ucb' (Upper Confidence Bound), 'pi' (Probability of Improvement), or 'ei' (Expected Improvement). - - In this formulation, higher predicted success probability is desired. - The GP model is assumed to provide predictions with uncertainties via its 'predict' method (using return_std=True). - - Parameters: - gp_model: A trained Gaussian Process model (e.g., GaussianProcessRegressor) that supports - predictions with uncertainties. - candidate_points (np.ndarray): Array of shape (n_candidates, d) representing candidate relative positions. - batch_size (int): Number of candidate samples to propose. - method (str): Acquisition function to use: 'ucb', 'pi', or 'ei'. Default is 'ucb'. - kappa (float): Exploration parameter for UCB. Default is 2.0. - xi (float): Exploration parameter for PI and EI. Default is 0.01. - - Returns: - best_candidates (np.ndarray): Array of shape (batch_size, d) containing the selected candidate points. - acquisition (np.ndarray): Acquisition values computed for each candidate point. - """ - # Obtain the predictive mean and standard deviation for each candidate point. - mu, sigma = gp_model.predict(candidate_points, return_std=True) - - # Compute the acquisition values based on the chosen method. - if method.lower() == "ucb": - # For maximization, UCB is defined as μ + kappa * σ. - acquisition = mu + kappa * sigma - elif method.lower() == "pi": - # Probability of Improvement (maximization formulation). - Z = (mu - 1.0 - xi) / (sigma + 1e-9) - acquisition = norm.cdf(Z) - elif method.lower() == "ei": - # Expected Improvement (maximization formulation). - Z = (mu - 1.0 - xi) / (sigma + 1e-9) - acquisition = (mu - 1.0 - xi) * norm.cdf(Z) + sigma * norm.pdf(Z) - # Handle nearly zero sigma values. - acquisition[sigma < 1e-9] = 0.0 - else: - raise ValueError("Unknown acquisition method. Please choose 'ucb', 'pi', or 'ei'.") - - # Sort candidates by acquisition value in descending order and select the top batch_size. - sorted_indices = np.argsort(acquisition)[::-1] - best_indices = sorted_indices[:batch_size] - best_candidates = candidate_points[best_indices] - - # Convert the numpy array to a torch tensor. - best_candidates_tensor = torch.from_numpy(best_candidates).to(device) - - return best_candidates_tensor, acquisition - - -""" -Util Functions -""" + parts = version_string.split(".") + major = int(parts[0]) + minor = int(parts[1]) if len(parts) > 1 else 0 + patch = int(parts[2]) if len(parts) > 2 else 0 + return (major, minor, patch) + + +def get_cuda_version(): + try: + # Execute nvcc --version command + result = subprocess.run(["nvcc", "--version"], capture_output=True, text=True, check=True) + output = result.stdout + + # Use regex to find the CUDA version (e.g., V11.2.67) + match = re.search(r"V(\d+\.\d+(\.\d+)?)", output) + if match: + return parse_cuda_version(match.group(1)) + else: + print("CUDA version not found in output.") + return None + except FileNotFoundError: + print("nvcc command not found. Is CUDA installed and in your PATH?") + return None + except subprocess.CalledProcessError as e: + print(f"Error executing nvcc: {e.stderr}") + return None + except Exception as e: + print(f"An unexpected error occurred: {e}") + return None def get_gripper_open_width(obj_filepath): diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py index fe292d31b4d..9308f281491 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_tasks_cfg.py @@ -118,7 +118,7 @@ class Extraction(DisassemblyTask): assembly_id = "00015" assembly_dir = f"{ASSET_DIR}/{assembly_id}/" disassembly_dir = "disassembly_dir" - num_log_traj = 1000 + num_log_traj = 100 fixed_asset_cfg = Hole8mm() held_asset_cfg = Peg8mm() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py index 01329d8ab70..4d1aab2e813 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/run_w_id.py @@ -59,11 +59,12 @@ def main(): update_task_param(args.cfg_path, args.assembly_id, args.train, args.log_eval) - bash_command = None + # avoid the warning of low GPU occupancy for SoftDTWCUDA function + bash_command = "NUMBA_CUDA_LOW_OCCUPANCY_WARNINGS=0" if sys.platform.startswith("win"): - bash_command = "isaaclab.bat -p" + bash_command += " isaaclab.bat -p" elif sys.platform.startswith("linux"): - bash_command = "./isaaclab.sh -p" + bash_command += " ./isaaclab.sh -p" if args.train: bash_command += " scripts/reinforcement_learning/rl_games/train.py --task=Isaac-AutoMate-Assembly-Direct-v0" bash_command += f" --seed={str(args.seed)} --max_iterations={str(args.max_iterations)}" diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py index f319a90e008..e3e74f0a075 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/soft_dtw_cuda.py @@ -120,11 +120,11 @@ class _SoftDTWCUDA(Function): """ @staticmethod - def forward(ctx, D, gamma, bandwidth): + def forward(ctx, D, device, gamma, bandwidth): dev = D.device dtype = D.dtype - gamma = torch.cuda.FloatTensor([gamma]) - bandwidth = torch.cuda.FloatTensor([bandwidth]) + gamma = torch.tensor([gamma], dtype=torch.float, device=device) + bandwidth = torch.tensor([bandwidth], dtype=torch.float, device=device) B = D.shape[0] N = D.shape[1] @@ -255,7 +255,7 @@ class _SoftDTW(Function): """ @staticmethod - def forward(ctx, D, gamma, bandwidth): + def forward(ctx, D, device, gamma, bandwidth): dev = D.device dtype = D.dtype gamma = torch.Tensor([gamma]).to(dev).type(dtype) # dtype fixed @@ -286,10 +286,11 @@ class SoftDTW(torch.nn.Module): The soft DTW implementation that optionally supports CUDA """ - def __init__(self, use_cuda, gamma=1.0, normalize=False, bandwidth=None, dist_func=None): + def __init__(self, use_cuda, device, gamma=1.0, normalize=False, bandwidth=None, dist_func=None): """ Initializes a new instance using the supplied parameters :param use_cuda: Flag indicating whether the CUDA implementation should be used + :param device: device to run the soft dtw computation :param gamma: sDTW's gamma parameter :param normalize: Flag indicating whether to perform normalization (as discussed in https://github.com/mblondel/soft-dtw/issues/10#issuecomment-383564790) @@ -301,6 +302,7 @@ def __init__(self, use_cuda, gamma=1.0, normalize=False, bandwidth=None, dist_fu self.gamma = gamma self.bandwidth = 0 if bandwidth is None else float(bandwidth) self.use_cuda = use_cuda + self.device = device # Set the distance function if dist_func is not None: @@ -357,12 +359,12 @@ def forward(self, X, Y): x = torch.cat([X, X, Y]) y = torch.cat([Y, X, Y]) D = self.dist_func(x, y) - out = func_dtw(D, self.gamma, self.bandwidth) + out = func_dtw(D, self.device, self.gamma, self.bandwidth) out_xy, out_xx, out_yy = torch.split(out, X.shape[0]) return out_xy - 1 / 2 * (out_xx + out_yy) else: D_xy = self.dist_func(X, Y) - return func_dtw(D_xy, self.gamma, self.bandwidth) + return func_dtw(D_xy, self.device, self.gamma, self.bandwidth) # ---------------------------------------------------------------------------------------------------------------------- diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml index 2ddc221af81..2f66ad8d20a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ippo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml index 7d9885205d4..ee30acb3484 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_mappo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml index cd8fff7ba72..c053b5b0035 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cart_double_pendulum/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml index 18719d99197..17fcf9c7271 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_camera_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: features_extractor - input: permute(STATES, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC layers: - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} @@ -36,7 +36,7 @@ models: clip_actions: False network: - name: features_extractor - input: permute(STATES, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC + input: permute(OBSERVATIONS, (0, 3, 1, 2)) # PyTorch NHWC -> NCHW. Warning: don't permute for JAX since it expects NHWC layers: - conv2d: {out_channels: 32, kernel_size: 8, stride: 4, padding: 0} - conv2d: {out_channels: 64, kernel_size: 4, stride: 2, padding: 0} diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml index 661acc55bad..83bcf50162a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml index 41a56f82fc2..d1cf5a6b5df 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_K1_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_K1_env.py new file mode 100644 index 00000000000..ab1761b394f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_K1_env.py @@ -0,0 +1,223 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab_assets import BOOSTER_K1_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, DeformableObjectCfg, DeformableObject, RigidObject, RigidObjectCfg, Articulation +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +import os + +from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv + + +@configclass +class BoosterK1EnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 15.0 + decimation = 2 + action_scale = 1.0 + action_space = 22 + observation_space = 79 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + terrain = TerrainImporterCfg( + prim_path="/World/Field", + terrain_type="usd", + usd_path= os.path.expanduser("~/IsaacLab-nomadz/source/isaaclab_assets/data/Environment/Field.usd"), + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="average", + restitution_combine_mode="average", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # Ball + ball_cfg = DeformableObjectCfg( + + prim_path="/World/envs/env_.*/Ball", + + spawn=sim_utils.UsdFileCfg( + + usd_path = os.path.expanduser( + "~/IsaacLab-nomadz/source/isaaclab_assets/data/Environment/Ball.usd"), + + deformable_props=sim_utils.DeformableBodyPropertiesCfg(rest_offset=0.0, contact_offset=0.001), + + mass_props= sim_utils.MassPropertiesCfg(mass=0.044, density=-1) + ), + + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.06)), + + debug_vis=False, + + ) + + # Goal blue + goal_blue_cfg = RigidObjectCfg( + prim_path = "/World/envs/env_.*/Goal_Blue", + + spawn=sim_utils.UsdFileCfg( + + usd_path = os.path.expanduser( + "~/IsaacLab-nomadz/source/isaaclab_assets/data/Environment/Goal_Blue.usd"), + + rigid_props = sim_utils.RigidBodyPropertiesCfg(), + mass_props = sim_utils.MassPropertiesCfg(mass=1.0), + collision_props = sim_utils.CollisionPropertiesCfg(), + ), + + init_state=RigidObjectCfg.InitialStateCfg(pos=(10.0, 0.0, 0.06), + rot=(0.671592, 0.120338, 0.712431, -0.164089) + ), + ) + + # Goal red + goal_red_cfg = RigidObjectCfg( + prim_path = "/World/envs/env_.*/Goal_Red", + + spawn=sim_utils.UsdFileCfg( + + usd_path = os.path.expanduser( + "~/IsaacLab-nomadz/source/isaaclab_assets/data/Environment/Goal_Red.usd"), + + rigid_props = sim_utils.RigidBodyPropertiesCfg(), + mass_props = sim_utils.MassPropertiesCfg(mass=1.0), + collision_props = sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-10.0, 0.0, 0.06), + rot=(0.523403, 0.677758, -0.513991, -0.080530) + ), + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=4.0, replicate_physics=True) + + # robot + robot: ArticulationCfg = BOOSTER_K1_CFG.replace(prim_path="/World/envs/env_.*/Robot") + + # TODO Correct these values + joint_gears: list = [ + 50.0, # AAHead_yaw (head - moderate) + 50.0, # Head_pitch (head - moderate) + + 40.0, # Left_Shoulder_Pitch (arms - lower effort) + 40.0, # Right_Shoulder_Pitch + + 40.0, # Left_Shoulder_Roll + 40.0, # Right_Shoulder_Roll + + 40.0, # Left_Elbow_Pitch + 40.0, # Right_Elbow_Pitch + + 40.0, # Left_Elbow_Yaw + 40.0, # Right_Elbow_Yaw + + + 45.0, # Left_Hip_Pitch (legs - high effort) + 45.0, # Right_Hip_Pitch + + 35.0, # Left_Hip_Roll + 35.0, # Right_Hip_Roll + + 35.0, # Left_Hip_Yaw + 35.0, # Right_Hip_Yaw + + 60.0, # Left_Knee_Pitch (highest effort) + 60.0, # Right_Knee_Pitch + + 25.0, # Left_Ankle_Pitch (feet - moderate) + 25.0, # Right_Ankle_Pitch + + 15.0, # Left_Ankle_Roll (feet - lower effort) + 15.0, # Right_Ankle_Roll + ] + + + heading_weight: float = 0.5 + up_weight: float = 0.1 + + energy_cost_scale: float = 0.05 + actions_cost_scale: float = 0.01 + alive_reward_scale: float = 2.0 + dof_vel_scale: float = 0.1 + + death_cost: float = -1.0 + termination_height: float = 0.8 + + angular_velocity_scale: float = 0.25 + contact_force_scale: float = 0.01 + + +class BoosterK1Env(LocomotionEnv): + cfg: BoosterK1EnvCfg + + def __init__(self, cfg: BoosterK1EnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + + + def _setup_scene(self): + # Instantiate robot and objects before cloning environments to the scene + self.robot = Articulation(self.cfg.robot) + self.ball = DeformableObject(self.cfg.ball_cfg) + self.goal_blue = RigidObject(self.cfg.goal_blue_cfg) + self.goal_red = RigidObject(self.cfg.goal_red_cfg) + + # add ground plane + self.cfg.terrain.num_envs = self.scene.cfg.num_envs + self.cfg.terrain.env_spacing = self.scene.cfg.env_spacing + self.terrain = self.cfg.terrain.class_type(self.cfg.terrain) + # clone and replicate + self.scene.clone_environments(copy_from_source=False) + # we need to explicitly filter collisions for CPU simulation + if self.device == "cpu": + self.scene.filter_collisions(global_prim_paths=[self.cfg.terrain.prim_path]) + + # add articulation to scene + self.scene.articulations["robot"] = self.robot + # add objects to scene + self.scene.deformable_objects["Ball"] = self.ball + self.scene.rigid_objects["Goal_Red"] = self.goal_red + self.scene.rigid_objects["Goal_Blue"] = self.goal_blue + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + + def _reset_idx(self, env_ids): + + super()._reset_idx(env_ids) + + # Reset Balls initial position + ball_state = self.ball.data.default_nodal_state_w + self.ball.write_nodal_state_to_sim(ball_state, env_ids=env_ids) + + # Reset goals position (Shouldnt be needed once floor is designed) + goal_blue_state = self.goal_blue.data.default_root_state + goal_red_state = self.goal_red.data.default_root_state + self.goal_blue.write_root_state_to_sim(goal_blue_state, env_ids) + self.goal_red.write_root_state_to_sim(goal_red_state, env_ids) + + + + diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_T1_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_T1_env.py new file mode 100644 index 00000000000..1cccdbcb493 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/Booster_T1_env.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from isaaclab_assets import BOOSTER_T1_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.direct.locomotion.locomotion_env import LocomotionEnv + + +@configclass +class BoosterT1EnvCfg(DirectRLEnvCfg): + # env + episode_length_s = 15.0 + decimation = 2 + action_scale = 1.0 + action_space = 23 + observation_space = 79 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation) + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="average", + restitution_combine_mode="average", + static_friction=1.0, + dynamic_friction=1.0, + restitution=0.0, + ), + debug_vis=False, + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=4096, env_spacing=4.0, replicate_physics=True) + + # robot + robot: ArticulationCfg = BOOSTER_T1_CFG.replace(prim_path="/World/envs/env_.*/Robot") + + # TODO Correct these values + joint_gears: list = [ + 50.0, # AAHead_yaw (head - moderate) + 50.0, # Head_pitch (head - moderate) + + 40.0, # Left_Shoulder_Pitch (arms - lower effort) + 40.0, # Right_Shoulder_Pitch + + 40.0, # Left_Shoulder_Roll + 40.0, # Right_Shoulder_Roll + + 40.0, # Left_Elbow_Pitch + 40.0, # Right_Elbow_Pitch + + 40.0, # Left_Elbow_Yaw + 40.0, # Right_Elbow_Yaw + + 30.0, # Waist (legs group, moderate effort) + + 45.0, # Left_Hip_Pitch (legs - high effort) + 45.0, # Right_Hip_Pitch + + 35.0, # Left_Hip_Roll + 35.0, # Right_Hip_Roll + + 35.0, # Left_Hip_Yaw + 35.0, # Right_Hip_Yaw + + 60.0, # Left_Knee_Pitch (highest effort) + 60.0, # Right_Knee_Pitch + + 25.0, # Left_Ankle_Pitch (feet - moderate) + 25.0, # Right_Ankle_Pitch + + 15.0, # Left_Ankle_Roll (feet - lower effort) + 15.0, # Right_Ankle_Roll + ] + + + heading_weight: float = 0.5 + up_weight: float = 0.1 + + energy_cost_scale: float = 0.05 + actions_cost_scale: float = 0.01 + alive_reward_scale: float = 2.0 + dof_vel_scale: float = 0.1 + + death_cost: float = -1.0 + termination_height: float = 0.8 + + angular_velocity_scale: float = 0.25 + contact_force_scale: float = 0.01 + + +class BoosterT1Env(LocomotionEnv): + cfg: BoosterT1EnvCfg + + def __init__(self, cfg: BoosterT1EnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py index ff38052a5cd..1a1da980642 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/__init__.py @@ -26,3 +26,27 @@ "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", }, ) + +gym.register( + id="Isaac-BoosterT1-Direct-v0", + entry_point=f"{__name__}.Booster_T1_env:BoosterT1Env", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.Booster_T1_env:BoosterT1EnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-BoosterK1-Direct-v0", + entry_point=f"{__name__}.Booster_K1_env:BoosterK1Env", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.Booster_K1_env:BoosterK1EnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml index aa0786091ee..130d1999ec3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml index 6b26961e3b6..090d5eb90a6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_dance_amp_cfg.yaml @@ -20,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -29,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -38,7 +38,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml index 4571db8777c..f74cecfeb64 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_run_amp_cfg.yaml @@ -20,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -29,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -38,7 +38,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml index 7cfa1dc367a..727258be3ca 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid_amp/agents/skrl_walk_amp_cfg.yaml @@ -20,7 +20,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -29,7 +29,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -38,7 +38,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml index bd7ac17eec0..3353c5786af 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml index 9d4da11bbbb..7ef224f78eb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ff_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 400, 200, 100] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml index d0d82c6c77e..cae9a8445e3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/feature_extractor.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/feature_extractor.py index 60a27649119..82d76ec7f1e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/feature_extractor.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/feature_extractor.py @@ -73,12 +73,13 @@ class FeatureExtractor: If the train flag is set to True, the CNN is trained during the rollout process. """ - def __init__(self, cfg: FeatureExtractorCfg, device: str): + def __init__(self, cfg: FeatureExtractorCfg, device: str, log_dir: str | None = None): """Initialize the feature extractor model. Args: - cfg (FeatureExtractorCfg): Configuration for the feature extractor model. - device (str): Device to run the model on. + cfg: Configuration for the feature extractor model. + device: Device to run the model on. + log_dir: Directory to save checkpoints. If None, uses local "logs" folder resolved with respect to this file. """ self.cfg = cfg @@ -89,7 +90,10 @@ def __init__(self, cfg: FeatureExtractorCfg, device: str): self.feature_extractor.to(self.device) self.step_count = 0 - self.log_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "logs") + if log_dir is not None: + self.log_dir = log_dir + else: + self.log_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "logs") if not os.path.exists(self.log_dir): os.makedirs(self.log_dir) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py index 6cde7d06fc1..42e8c4f03c4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py @@ -65,7 +65,8 @@ class ShadowHandVisionEnv(InHandManipulationEnv): def __init__(self, cfg: ShadowHandVisionEnvCfg, render_mode: str | None = None, **kwargs): super().__init__(cfg, render_mode, **kwargs) - self.feature_extractor = FeatureExtractor(self.cfg.feature_extractor, self.device) + # Use the log directory from the configuration + self.feature_extractor = FeatureExtractor(self.cfg.feature_extractor, self.device, self.cfg.log_dir) # hide goal cubes self.goal_pos[:, :] = torch.tensor([-0.2, 0.1, 0.6], device=self.device) # keypoints buffer diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml index c9bf684b008..84f23d446f6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ippo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml index 7dd38e3096d..479219a8628 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_mappo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml index 38b8f6ce014..789738bdf90 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml index 9cae13d9b22..003ec762be5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/sb3_ppo_cfg.yaml @@ -6,19 +6,19 @@ # Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L161 seed: 42 -n_timesteps: !!float 1e7 +n_timesteps: !!float 1e8 policy: 'MlpPolicy' -batch_size: 128 -n_steps: 512 +batch_size: 32768 +n_steps: 16 gamma: 0.99 gae_lambda: 0.9 -n_epochs: 20 +n_epochs: 4 ent_coef: 0.0 sde_sample_freq: 4 max_grad_norm: 0.5 vf_coef: 0.5 learning_rate: !!float 3e-5 -use_sde: True +use_sde: False clip_range: 0.4 device: "cuda:0" policy_kwargs: diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml index 48eaa50c03c..4375afee0cb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml index d5c8157ce35..4a2b308e670 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml index d471c535f91..e9f3913a029 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [400, 200, 100] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py new file mode 100644 index 00000000000..739fdf113e6 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" + +from .tracking import * # noqa diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py new file mode 100644 index 00000000000..a3b30988b7f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py @@ -0,0 +1,31 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" + +import gymnasium as gym +import os + +from . import agents, fixed_base_upper_body_ik_g1_env_cfg, locomanipulation_g1_env_cfg + +gym.register( + id="Isaac-PickPlace-Locomanipulation-G1-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": locomanipulation_g1_env_cfg.LocomanipulationG1EnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": fixed_base_upper_body_ik_g1_env_cfg.FixedBaseUpperBodyIKG1EnvCfg, + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 00000000000..c1dce5f832c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json @@ -0,0 +1,117 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_low_dim_g1", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 100, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "dataset_keys": [ + "actions" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 2000, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state", + "object" + ], + "rgb": [], + "depth": [], + "scan": [] + } + } + } +} diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py new file mode 100644 index 00000000000..4d8db0b0c15 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from ..mdp.actions import AgileBasedLowerBodyAction + + +@configclass +class AgileBasedLowerBodyActionCfg(ActionTermCfg): + """Configuration for the lower body action term that is based on Agile lower body RL policy.""" + + class_type: type[ActionTerm] = AgileBasedLowerBodyAction + """The class type for the lower body action term.""" + + joint_names: list[str] = MISSING + """The names of the joints to control.""" + + obs_group_name: str = MISSING + """The name of the observation group to use.""" + + policy_path: str = MISSING + """The path to the policy model.""" + + policy_output_offset: float = 0.0 + """Offsets the output of the policy.""" + + policy_output_scale: float = 1.0 + """Scales the output of the policy.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py new file mode 100644 index 00000000000..e4e22987442 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py @@ -0,0 +1,84 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.envs import mdp +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + + +@configclass +class AgileTeacherPolicyObservationsCfg(ObsGroup): + """Observation specifications for the Agile lower body policy. + + Note: This configuration defines only part of the observation input to the Agile lower body policy. + The lower body command portion is appended to the observation tensor in the action term, as that + is where the environment has access to those commands. + """ + + base_lin_vel = ObsTerm( + func=mdp.base_lin_vel, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + base_ang_vel = ObsTerm( + func=mdp.base_ang_vel, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + projected_gravity = ObsTerm( + func=mdp.projected_gravity, + scale=1.0, + ) + + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_.*_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + "waist_.*_joint", + ], + ), + }, + ) + + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + scale=0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_.*_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + "waist_.*_joint", + ], + ), + }, + ) + + actions = ObsTerm( + func=mdp.last_action, + scale=1.0, + params={ + "action_name": "lower_body_joint_pos", + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py new file mode 100644 index 00000000000..1c80674e383 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py @@ -0,0 +1,126 @@ +# Copyright (c) 2022-2025, 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 pink controller. + +This module provides configurations for humanoid robot pink IK controllers, +including both fixed base and mobile configurations for upper body manipulation. +""" + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg + +## +# Pink IK Controller Configuration for G1 +## + +G1_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=14, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + "g1_29dof_with_hand_rev_1_0_left_wrist_yaw_link", + base_link_frame_name="g1_29dof_with_hand_rev_1_0_pelvis", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + LocalFrameTask( + "g1_29dof_with_hand_rev_1_0_right_wrist_yaw_link", + base_link_frame_name="g1_29dof_with_hand_rev_1_0_pelvis", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "g1_29dof_with_hand_rev_1_0_left_wrist_yaw_link", + "g1_29dof_with_hand_rev_1_0_right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], +) +"""Base configuration for the G1 pink IK controller. + +This configuration sets up the pink IK controller for the G1 humanoid robot with +left and right wrist control tasks. The controller is designed for upper body +manipulation tasks. +""" + + +## +# Pink IK Action Configuration for G1 +## + +G1_UPPER_BODY_IK_ACTION_CFG = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_pitch_joint", + ".*_wrist_roll_joint", + ".*_wrist_yaw_joint", + "waist_.*_joint", + ], + hand_joint_names=[ + "left_hand_index_0_joint", # Index finger proximal + "left_hand_middle_0_joint", # Middle finger proximal + "left_hand_thumb_0_joint", # Thumb base (yaw axis) + "right_hand_index_0_joint", # Index finger proximal + "right_hand_middle_0_joint", # Middle finger proximal + "right_hand_thumb_0_joint", # Thumb base (yaw axis) + "left_hand_index_1_joint", # Index finger distal + "left_hand_middle_1_joint", # Middle finger distal + "left_hand_thumb_1_joint", # Thumb middle (pitch axis) + "right_hand_index_1_joint", # Index finger distal + "right_hand_middle_1_joint", # Middle finger distal + "right_hand_thumb_1_joint", # Thumb middle (pitch axis) + "left_hand_thumb_2_joint", # Thumb tip + "right_hand_thumb_2_joint", # Thumb tip + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=G1_UPPER_BODY_IK_CONTROLLER_CFG, +) +"""Base configuration for the G1 pink IK action. + +This configuration sets up the pink IK action for the G1 humanoid robot, +defining which joints are controlled by the IK solver and which are fixed. +The configuration includes: +- Upper body joints controlled by IK (shoulders, elbows, wrists) +- Fixed joints (pelvis, legs, hands) +- Hand joint names for additional control +- Reference to the pink IK controller configuration +""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py new file mode 100644 index 00000000000..e3ace99b520 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py @@ -0,0 +1,215 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab_assets.robots.unitree import G1_29DOF_CFG + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, +) + + +## +# Scene definition +## +@configclass +class FixedBaseUpperBodyIKG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for fixed base upper body IK environment with G1 robot. + + This configuration sets up the G1 humanoid robot with fixed pelvis and legs, + allowing only arm manipulation while the base remains stationary. The robot is + controlled using upper body IK. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Unitree G1 Humanoid robot - fixed base configuration + robot: ArticulationCfg = G1_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + def __post_init__(self): + """Post initialization.""" + # Set the robot to fixed base + self.robot.spawn.articulation_props.fix_root_link = True + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_UPPER_BODY_IK_ACTION_CFG + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + head_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": []}) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class FixedBaseUpperBodyIKG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 fixed base upper body IK environment. + + This environment is designed for manipulation tasks where the G1 humanoid robot + has a fixed pelvis and legs, allowing only arm and hand movements for manipulation. The robot is + controlled using upper body IK. + """ + + # Scene settings + scene: FixedBaseUpperBodyIKG1SceneCfg = FixedBaseUpperBodyIKG1SceneCfg( + num_envs=1, env_spacing=2.5, replicate_physics=True + ) + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, -0.45), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 2 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyRetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py new file mode 100644 index 00000000000..bf09c7f0426 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -0,0 +1,229 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab_assets.robots.unitree import G1_29DOF_CFG + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeterCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.action_cfg import AgileBasedLowerBodyActionCfg +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.agile_locomotion_observation_cfg import ( + AgileTeacherPolicyObservationsCfg, +) +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, +) + + +## +# Scene definition +## +@configclass +class LocomanipulationG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for locomanipulation environment with G1 robot. + + This configuration sets up the G1 humanoid robot for locomanipulation tasks, + allowing both locomotion and manipulation capabilities. The robot can move its + base and use its arms for manipulation tasks. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = G1_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_UPPER_BODY_IK_ACTION_CFG + + lower_body_joint_pos = AgileBasedLowerBodyActionCfg( + asset_name="robot", + joint_names=[ + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + ], + policy_output_scale=0.25, + obs_group_name="lower_body_policy", # need to be the same name as the on in ObservationCfg + policy_path=f"{ISAACLAB_NUCLEUS_DIR}/Policies/Agile/agile_locomotion.pt", + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + lower_body_policy: AgileTeacherPolicyObservationsCfg = AgileTeacherPolicyObservationsCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 locomanipulation environment. + + This environment is designed for locomanipulation tasks where the G1 humanoid robot + can perform both locomotion and manipulation simultaneously. The robot can move its + base and use its arms for manipulation tasks, enabling complex mobile manipulation + behaviors. + """ + + # Scene settings + scene: LocomanipulationG1SceneCfg = LocomanipulationG1SceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # MDP settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands = None + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, -0.35), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 2 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyRetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingRetargeterCfg( + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py new file mode 100644 index 00000000000..18ec38070d5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .actions import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py new file mode 100644 index 00000000000..ad0384a5b82 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py @@ -0,0 +1,125 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.io.torchscript import load_torchscript_model + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .configs.action_cfg import AgileBasedLowerBodyActionCfg + + +class AgileBasedLowerBodyAction(ActionTerm): + """Action term that is based on Agile lower body RL policy.""" + + cfg: AgileBasedLowerBodyActionCfg + """The configuration of the action term.""" + + _asset: Articulation + """The articulation asset to which the action term is applied.""" + + def __init__(self, cfg: AgileBasedLowerBodyActionCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + + # Save the observation config from cfg + self._observation_cfg = env.cfg.observations + self._obs_group_name = cfg.obs_group_name + + # Load policy here if needed + _temp_policy_path = retrieve_file_path(cfg.policy_path) + self._policy = load_torchscript_model(_temp_policy_path, device=env.device) + self._env = env + + # Find joint ids for the lower body joints + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + + # Get the scale and offset from the configuration + self._policy_output_scale = torch.tensor(cfg.policy_output_scale, device=env.device) + self._policy_output_offset = self._asset.data.default_joint_pos[:, self._joint_ids].clone() + + # Create tensors to store raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, len(self._joint_ids), device=self.device) + self._processed_actions = torch.zeros(self.num_envs, len(self._joint_ids), device=self.device) + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Lower Body Action: [vx, vy, wz, hip_height]""" + return 4 + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + def _compose_policy_input(self, base_command: torch.Tensor, obs_tensor: torch.Tensor) -> torch.Tensor: + """Compose the policy input by concatenating repeated commands with observations. + + Args: + base_command: The base command tensor [vx, vy, wz, hip_height]. + obs_tensor: The observation tensor from the environment. + + Returns: + The composed policy input tensor with repeated commands concatenated to observations. + """ + # Get history length from observation configuration + history_length = getattr(self._observation_cfg, self._obs_group_name).history_length + # Default to 1 if history_length is None (no history, just current observation) + if history_length is None: + history_length = 1 + + # Repeat commands based on history length and concatenate with observations + repeated_commands = base_command.unsqueeze(1).repeat(1, history_length, 1).reshape(base_command.shape[0], -1) + policy_input = torch.cat([repeated_commands, obs_tensor], dim=-1) + + return policy_input + + def process_actions(self, actions: torch.Tensor): + """Process the input actions using the locomotion policy. + + Args: + actions: The lower body commands. + """ + + # Extract base command from the action tensor + # Assuming the base command [vx, vy, wz, hip_height] + base_command = actions + + obs_tensor = self._env.obs_buf["lower_body_policy"] + + # Compose policy input using helper function + policy_input = self._compose_policy_input(base_command, obs_tensor) + + joint_actions = self._policy.forward(policy_input) + + self._raw_actions[:] = joint_actions + + # Apply scaling and offset to the raw actions from the policy + self._processed_actions = joint_actions * self._policy_output_scale + self._policy_output_offset + + # Clip actions if configured + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def apply_actions(self): + """Apply the actions to the environment.""" + # Store the raw actions + self._asset.set_joint_position_target(self._processed_actions, joint_ids=self._joint_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py new file mode 100644 index 00000000000..ab027ce0bf1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2025, 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 torch + +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import SceneEntityCfg + + +def upper_body_last_action( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Extract the last action of the upper body.""" + asset = env.scene[asset_cfg.name] + joint_pos_target = asset.data.joint_pos_target + + # Use joint_names from asset_cfg to find indices + joint_names = asset_cfg.joint_names if hasattr(asset_cfg, "joint_names") else None + if joint_names is None: + raise ValueError("asset_cfg must have 'joint_names' attribute for upper_body_last_action.") + + # Find joint indices matching the provided joint_names (supports regex) + joint_indices, _ = asset.find_joints(joint_names) + joint_indices = torch.tensor(joint_indices, dtype=torch.long) + + # Get upper body joint positions for all environments + upper_body_joint_pos_target = joint_pos_target[:, joint_indices] + + return upper_body_joint_pos_target diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/loco_manip_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/loco_manip_env_cfg.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/loco_manip_env_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/loco_manip_env_cfg.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml index 3ef50e08dcc..873657e3578 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml index 7c4577efc4e..b8227096f5d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml index e6c7fdc17c0..d8c336da407 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml index 4ea1d0a4044..2273df9c37d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml index e8fb16d26cb..f0942278b83 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml index 3c929fa0ee8..5c7fedf07b0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml index 33627d76a3e..88a2bc75b25 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml index ea54efbb14e..9df85573ef5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml index 43ddef1bcd7..dd80f5fd196 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml index db92e1f86ce..883148f878e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml index 3aa08627382..b6ecdf1f301 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml index 3d9390bf722..6013e3f070d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml index 51445b2aadb..7cd7c9bb5b5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml index cbd8389751c..79daaec43f2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml index e7be95a9196..1b3ecf74fd5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml index 4fef61da4a3..aeffb439a17 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml index a6166fcb1d3..1bcc39eb42e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml index d111bdc8024..7538f906a21 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/skrl_rough_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml index 104e205d4b6..c380e841e4c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml index 341db684146..4e81f3673de 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/__init__.py new file mode 100644 index 00000000000..26075d4da25 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/__init__.py @@ -0,0 +1,26 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Dexsuite environments. + +Implementation Reference: + +Reorient: +@article{petrenko2023dexpbt, + title={Dexpbt: Scaling up dexterous manipulation for hand-arm systems with population based training}, + author={Petrenko, Aleksei and Allshire, Arthur and State, Gavriel and Handa, Ankur and Makoviychuk, Viktor}, + journal={arXiv preprint arXiv:2305.12127}, + year={2023} +} + +Lift: +@article{singh2024dextrah, + title={Dextrah-rgb: Visuomotor policies to grasp anything with dexterous hands}, + author={Singh, Ritvik and Allshire, Arthur and Handa, Ankur and Ratliff, Nathan and Van Wyk, Karl}, + journal={arXiv preprint arXiv:2412.01791}, + year={2024} +} + +""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/adr_curriculum.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/adr_curriculum.py new file mode 100644 index 00000000000..52fef8b494a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/adr_curriculum.py @@ -0,0 +1,122 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.utils import configclass + +from . import mdp + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + # adr stands for automatic/adaptive domain randomization + adr = CurrTerm( + func=mdp.DifficultyScheduler, params={"init_difficulty": 0, "min_difficulty": 0, "max_difficulty": 10} + ) + + joint_pos_unoise_min_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.joint_pos.noise.n_min", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.1, "difficulty_term_str": "adr"}, + }, + ) + + joint_pos_unoise_max_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.joint_pos.noise.n_max", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": 0.1, "difficulty_term_str": "adr"}, + }, + ) + + joint_vel_unoise_min_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.joint_vel.noise.n_min", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.2, "difficulty_term_str": "adr"}, + }, + ) + + joint_vel_unoise_max_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.joint_vel.noise.n_max", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": 0.2, "difficulty_term_str": "adr"}, + }, + ) + + hand_tips_pos_unoise_min_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.hand_tips_state_b.noise.n_min", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.01, "difficulty_term_str": "adr"}, + }, + ) + + hand_tips_pos_unoise_max_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.proprio.hand_tips_state_b.noise.n_max", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": 0.01, "difficulty_term_str": "adr"}, + }, + ) + + object_quat_unoise_min_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.policy.object_quat_b.noise.n_min", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.03, "difficulty_term_str": "adr"}, + }, + ) + + object_quat_unoise_max_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.policy.object_quat_b.noise.n_max", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": 0.03, "difficulty_term_str": "adr"}, + }, + ) + + object_obs_unoise_min_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.perception.object_point_cloud.noise.n_min", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.01, "difficulty_term_str": "adr"}, + }, + ) + + object_obs_unoise_max_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "observations.perception.object_point_cloud.noise.n_max", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": {"initial_value": 0.0, "final_value": -0.01, "difficulty_term_str": "adr"}, + }, + ) + + gravity_adr = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "events.variable_gravity.params.gravity_distribution_params", + "modify_fn": mdp.initial_final_interpolate_fn, + "modify_params": { + "initial_value": ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), + "final_value": ((0.0, 0.0, -9.81), (0.0, 0.0, -9.81)), + "difficulty_term_str": "adr", + }, + }, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/__init__.py new file mode 100644 index 00000000000..4240e604428 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the dexsuite environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/__init__.py new file mode 100644 index 00000000000..159ab6727fb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/__init__.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Dextra Kuka Allegro environments. +""" + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +# State Observation +gym.register( + id="Isaac-Dexsuite-Kuka-Allegro-Reorient-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.dexsuite_kuka_allegro_env_cfg:DexsuiteKukaAllegroReorientEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DexsuiteKukaAllegroPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Dexsuite-Kuka-Allegro-Reorient-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.dexsuite_kuka_allegro_env_cfg:DexsuiteKukaAllegroReorientEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DexsuiteKukaAllegroPPORunnerCfg", + }, +) + +# Dexsuite Lift Environments +gym.register( + id="Isaac-Dexsuite-Kuka-Allegro-Lift-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.dexsuite_kuka_allegro_env_cfg:DexsuiteKukaAllegroLiftEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DexsuiteKukaAllegroPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Dexsuite-Kuka-Allegro-Lift-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.dexsuite_kuka_allegro_env_cfg:DexsuiteKukaAllegroLiftEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DexsuiteKukaAllegroPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/agents/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..f4ac23fcd7a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,111 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + obs_groups: + obs: ["policy", "proprio", "perception"] + states: ["policy", "proprio", "perception"] + concate_obs_groups: True + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: True + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [512, 256, 128] + activation: elu + d2rl: False + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: reorient + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: False + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-3 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 100000000 + max_epochs: 750000 + save_best_after: 100 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.001 + truncate_grads: True + e_clip: 0.2 + horizon_length: 36 + minibatch_size: 36864 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + clip_actions: False + seq_len: 4 + bounds_loss_coef: 0.0001 + +pbt: + enabled: False + policy_idx: 0 # policy index in a population + num_policies: 8 # total number of policies in the population + directory: . + workspace: "pbt_workspace" # suffix of the workspace dir name inside train_dir + objective: episode.Curriculum/adr + + # PBT hyperparams + interval_steps: 50000000 + threshold_std: 0.1 + threshold_abs: 0.025 + mutation_rate: 0.25 + change_range: [1.1, 2.0] + mutation: + + agent.params.config.learning_rate: "mutate_float" + agent.params.config.grad_norm: "mutate_float" + agent.params.config.entropy_coef: "mutate_float" + agent.params.config.critic_coef: "mutate_float" + agent.params.config.bounds_loss_coef: "mutate_float" + agent.params.config.kl_threshold: "mutate_float" + agent.params.config.gamma: "mutate_discount" + agent.params.config.tau: "mutate_discount" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..f7965575737 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class DexsuiteKukaAllegroPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 32 + obs_groups = {"policy": ["policy", "proprio", "perception"], "critic": ["policy", "proprio", "perception"]} + max_iterations = 15000 + save_interval = 250 + experiment_name = "dexsuite_kuka_allegro" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=True, + critic_obs_normalization=True, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py new file mode 100644 index 00000000000..6c41414f30b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab_assets.robots import KUKA_ALLEGRO_CFG + +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensorCfg +from isaaclab.utils import configclass + +from ... import dexsuite_env_cfg as dexsuite +from ... import mdp + + +@configclass +class KukaAllegroRelJointPosActionCfg: + action = mdp.RelativeJointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.1) + + +@configclass +class KukaAllegroReorientRewardCfg(dexsuite.RewardsCfg): + + # bool awarding term if 2 finger tips are in contact with object, one of the contacting fingers has to be thumb. + good_finger_contact = RewTerm( + func=mdp.contacts, + weight=0.5, + params={"threshold": 1.0}, + ) + + +@configclass +class KukaAllegroMixinCfg: + rewards: KukaAllegroReorientRewardCfg = KukaAllegroReorientRewardCfg() + actions: KukaAllegroRelJointPosActionCfg = KukaAllegroRelJointPosActionCfg() + + def __post_init__(self: dexsuite.DexsuiteReorientEnvCfg): + super().__post_init__() + self.commands.object_pose.body_name = "palm_link" + self.scene.robot = KUKA_ALLEGRO_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + finger_tip_body_list = ["index_link_3", "middle_link_3", "ring_link_3", "thumb_link_3"] + for link_name in finger_tip_body_list: + setattr( + self.scene, + f"{link_name}_object_s", + ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/ee_link/" + link_name, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Object"], + ), + ) + self.observations.proprio.contact = ObsTerm( + func=mdp.fingers_contact_force_b, + params={"contact_sensor_names": [f"{link}_object_s" for link in finger_tip_body_list]}, + clip=(-20.0, 20.0), # contact force in finger tips is under 20N normally + ) + self.observations.proprio.hand_tips_state_b.params["body_asset_cfg"].body_names = ["palm_link", ".*_tip"] + self.rewards.fingers_to_object.params["asset_cfg"] = SceneEntityCfg("robot", body_names=["palm_link", ".*_tip"]) + + +@configclass +class DexsuiteKukaAllegroReorientEnvCfg(KukaAllegroMixinCfg, dexsuite.DexsuiteReorientEnvCfg): + pass + + +@configclass +class DexsuiteKukaAllegroReorientEnvCfg_PLAY(KukaAllegroMixinCfg, dexsuite.DexsuiteReorientEnvCfg_PLAY): + pass + + +@configclass +class DexsuiteKukaAllegroLiftEnvCfg(KukaAllegroMixinCfg, dexsuite.DexsuiteLiftEnvCfg): + pass + + +@configclass +class DexsuiteKukaAllegroLiftEnvCfg_PLAY(KukaAllegroMixinCfg, dexsuite.DexsuiteLiftEnvCfg_PLAY): + pass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py new file mode 100644 index 00000000000..75e40c5c74b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py @@ -0,0 +1,466 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedEnvCfg, ViewerCfg +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.sim import CapsuleCfg, ConeCfg, CuboidCfg, RigidBodyMaterialCfg, SphereCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +from . import mdp +from .adr_curriculum import CurriculumCfg + + +@configclass +class SceneCfg(InteractiveSceneCfg): + """Dexsuite Scene for multi-objects Lifting""" + + # robot + robot: ArticulationCfg = MISSING + + # object + object: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + CuboidCfg(size=(0.05, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CuboidCfg(size=(0.05, 0.05, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CuboidCfg(size=(0.025, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CuboidCfg(size=(0.025, 0.05, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CuboidCfg(size=(0.025, 0.025, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CuboidCfg(size=(0.01, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + SphereCfg(radius=0.05, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + SphereCfg(radius=0.025, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.04, height=0.025, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.04, height=0.01, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.04, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.025, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.025, height=0.2, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + CapsuleCfg(radius=0.01, height=0.2, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + ConeCfg(radius=0.05, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + ConeCfg(radius=0.025, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + ], + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=0, + disable_gravity=False, + ), + collision_props=sim_utils.CollisionPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.2), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.55, 0.1, 0.35)), + ) + + # table + table: RigidObjectCfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/table", + spawn=sim_utils.CuboidCfg( + size=(0.8, 1.5, 0.04), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + collision_props=sim_utils.CollisionPropertiesCfg(), + # trick: we let visualizer's color to show the table with success coloring + visible=False, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.55, 0.0, 0.235), rot=(1.0, 0.0, 0.0, 0.0)), + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(), + spawn=sim_utils.GroundPlaneCfg(), + collision_group=-1, + ) + + # lights + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + object_pose = mdp.ObjectUniformPoseCommandCfg( + asset_name="robot", + object_name="object", + resampling_time_range=(3.0, 5.0), + debug_vis=False, + ranges=mdp.ObjectUniformPoseCommandCfg.Ranges( + pos_x=(-0.7, -0.3), + pos_y=(-0.25, 0.25), + pos_z=(0.55, 0.95), + roll=(-3.14, 3.14), + pitch=(-3.14, 3.14), + yaw=(0.0, 0.0), + ), + success_vis_asset_name="table", + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + object_quat_b = ObsTerm(func=mdp.object_quat_b, noise=Unoise(n_min=-0.0, n_max=0.0)) + target_object_pose_b = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + self.history_length = 5 + + @configclass + class ProprioObsCfg(ObsGroup): + """Observations for proprioception group.""" + + joint_pos = ObsTerm(func=mdp.joint_pos, noise=Unoise(n_min=-0.0, n_max=0.0)) + joint_vel = ObsTerm(func=mdp.joint_vel, noise=Unoise(n_min=-0.0, n_max=0.0)) + hand_tips_state_b = ObsTerm( + func=mdp.body_state_b, + noise=Unoise(n_min=-0.0, n_max=0.0), + # good behaving number for position in m, velocity in m/s, rad/s, + # and quaternion are unlikely to exceed -2 to 2 range + clip=(-2.0, 2.0), + params={ + "body_asset_cfg": SceneEntityCfg("robot"), + "base_asset_cfg": SceneEntityCfg("robot"), + }, + ) + contact: ObsTerm = MISSING + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + self.history_length = 5 + + @configclass + class PerceptionObsCfg(ObsGroup): + + object_point_cloud = ObsTerm( + func=mdp.object_point_cloud_b, + noise=Unoise(n_min=-0.0, n_max=0.0), + clip=(-2.0, 2.0), # clamp between -2 m to 2 m + params={"num_points": 64, "flatten": True}, + ) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_dim = 0 + self.concatenate_terms = True + self.flatten_history_dim = True + self.history_length = 5 + + # observation groups + policy: PolicyCfg = PolicyCfg() + proprio: ProprioObsCfg = ProprioObsCfg() + perception: PerceptionObsCfg = PerceptionObsCfg() + + +@configclass +class EventCfg: + """Configuration for randomization.""" + + # -- pre-startup + randomize_object_scale = EventTerm( + func=mdp.randomize_rigid_body_scale, + mode="prestartup", + params={"scale_range": (0.75, 1.5), "asset_cfg": SceneEntityCfg("object")}, + ) + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": [0.5, 1.0], + "dynamic_friction_range": [0.5, 1.0], + "restitution_range": [0.0, 0.0], + "num_buckets": 250, + }, + ) + + object_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("object", body_names=".*"), + "static_friction_range": [0.5, 1.0], + "dynamic_friction_range": [0.5, 1.0], + "restitution_range": [0.0, 0.0], + "num_buckets": 250, + }, + ) + + joint_stiffness_and_damping = EventTerm( + func=mdp.randomize_actuator_gains, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "stiffness_distribution_params": [0.5, 2.0], + "damping_distribution_params": [0.5, 2.0], + "operation": "scale", + }, + ) + + joint_friction = EventTerm( + func=mdp.randomize_joint_parameters, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=".*"), + "friction_distribution_params": [0.0, 5.0], + "operation": "scale", + }, + ) + + object_scale_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("object"), + "mass_distribution_params": [0.2, 2.0], + "operation": "scale", + }, + ) + + reset_table = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": [-0.05, 0.05], "y": [-0.05, 0.05], "z": [0.0, 0.0]}, + "velocity_range": {"x": [-0.0, 0.0], "y": [-0.0, 0.0], "z": [-0.0, 0.0]}, + "asset_cfg": SceneEntityCfg("table"), + }, + ) + + reset_object = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.2, 0.2], + "y": [-0.2, 0.2], + "z": [0.0, 0.4], + "roll": [-3.14, 3.14], + "pitch": [-3.14, 3.14], + "yaw": [-3.14, 3.14], + }, + "velocity_range": {"x": [-0.0, 0.0], "y": [-0.0, 0.0], "z": [-0.0, 0.0]}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + reset_root = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": [-0.0, 0.0], "y": [-0.0, 0.0], "yaw": [-0.0, 0.0]}, + "velocity_range": {"x": [-0.0, 0.0], "y": [-0.0, 0.0], "z": [-0.0, 0.0]}, + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "position_range": [-0.50, 0.50], + "velocity_range": [0.0, 0.0], + }, + ) + + reset_robot_wrist_joint = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names="iiwa7_joint_7"), + "position_range": [-3, 3], + "velocity_range": [0.0, 0.0], + }, + ) + + # Note (Octi): This is a deliberate trick in Remake to accelerate learning. + # By scheduling gravity as a curriculum — starting with no gravity (easy) + # and gradually introducing full gravity (hard) — the agent learns more smoothly. + # This removes the need for a special "Lift" reward (often required to push the + # agent to counter gravity), which has bonus effect of simplifying reward composition overall. + variable_gravity = EventTerm( + func=mdp.randomize_physics_scene_gravity, + mode="reset", + params={ + "gravity_distribution_params": ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), + "operation": "abs", + }, + ) + + +@configclass +class ActionsCfg: + pass + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + action_l2 = RewTerm(func=mdp.action_l2_clamped, weight=-0.005) + + action_rate_l2 = RewTerm(func=mdp.action_rate_l2_clamped, weight=-0.005) + + fingers_to_object = RewTerm(func=mdp.object_ee_distance, params={"std": 0.4}, weight=1.0) + + position_tracking = RewTerm( + func=mdp.position_command_error_tanh, + weight=2.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 0.2, + "command_name": "object_pose", + "align_asset_cfg": SceneEntityCfg("object"), + }, + ) + + orientation_tracking = RewTerm( + func=mdp.orientation_command_error_tanh, + weight=4.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 1.5, + "command_name": "object_pose", + "align_asset_cfg": SceneEntityCfg("object"), + }, + ) + + success = RewTerm( + func=mdp.success_reward, + weight=10, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "pos_std": 0.1, + "rot_std": 0.5, + "command_name": "object_pose", + "align_asset_cfg": SceneEntityCfg("object"), + }, + ) + + early_termination = RewTerm(func=mdp.is_terminated_term, weight=-1, params={"term_keys": "abnormal_robot"}) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_out_of_bound = DoneTerm( + func=mdp.out_of_bound, + params={ + "in_bound_range": {"x": (-1.5, 0.5), "y": (-2.0, 2.0), "z": (0.0, 2.0)}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + abnormal_robot = DoneTerm(func=mdp.abnormal_robot_state) + + +@configclass +class DexsuiteReorientEnvCfg(ManagerBasedEnvCfg): + """Dexsuite reorientation task definition, also the base definition for derivative Lift task and evaluation task""" + + # Scene settings + viewer: ViewerCfg = ViewerCfg(eye=(-2.25, 0.0, 0.75), lookat=(0.0, 0.0, 0.45), origin_type="env") + scene: SceneCfg = SceneCfg(num_envs=4096, env_spacing=3, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg | None = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 # 50 Hz + + # *single-goal setup + self.commands.object_pose.resampling_time_range = (10.0, 10.0) + self.commands.object_pose.position_only = False + self.commands.object_pose.success_visualizer_cfg.markers["failure"] = self.scene.table.spawn.replace( + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.25, 0.15, 0.15), roughness=0.25), visible=True + ) + self.commands.object_pose.success_visualizer_cfg.markers["success"] = self.scene.table.spawn.replace( + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.15, 0.25, 0.15), roughness=0.25), visible=True + ) + + self.episode_length_s = 4.0 + self.is_finite_horizon = True + + # simulation settings + self.sim.dt = 1 / 120 + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_max_rigid_patch_count = 4 * 5 * 2**15 + + if self.curriculum is not None: + self.curriculum.adr.params["pos_tol"] = self.rewards.success.params["pos_std"] / 2 + self.curriculum.adr.params["rot_tol"] = self.rewards.success.params["rot_std"] / 2 + + +class DexsuiteLiftEnvCfg(DexsuiteReorientEnvCfg): + """Dexsuite lift task definition""" + + def __post_init__(self): + super().__post_init__() + self.rewards.orientation_tracking = None # no orientation reward + self.commands.object_pose.position_only = True + if self.curriculum is not None: + self.rewards.success.params["rot_std"] = None # make success reward not consider orientation + self.curriculum.adr.params["rot_tol"] = None # make adr not tracking orientation + + +class DexsuiteReorientEnvCfg_PLAY(DexsuiteReorientEnvCfg): + """Dexsuite reorientation task evaluation environment definition""" + + def __post_init__(self): + super().__post_init__() + self.commands.object_pose.resampling_time_range = (2.0, 3.0) + self.commands.object_pose.debug_vis = True + self.curriculum.adr.params["init_difficulty"] = self.curriculum.adr.params["max_difficulty"] + + +class DexsuiteLiftEnvCfg_PLAY(DexsuiteLiftEnvCfg): + """Dexsuite lift task evaluation environment definition""" + + def __post_init__(self): + super().__post_init__() + self.commands.object_pose.resampling_time_range = (2.0, 3.0) + self.commands.object_pose.debug_vis = True + self.commands.object_pose.position_only = True + self.curriculum.adr.params["init_difficulty"] = self.curriculum.adr.params["max_difficulty"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/__init__.py new file mode 100644 index 00000000000..794113f9253 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .commands import * # noqa: F401, F403 +from .curriculums import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/__init__.py new file mode 100644 index 00000000000..a5132558174 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .pose_commands_cfg import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands.py new file mode 100644 index 00000000000..146eee9741d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands.py @@ -0,0 +1,179 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Sub-module containing command generators for pose tracking.""" + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import CommandTerm +from isaaclab.markers import VisualizationMarkers +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error, quat_from_euler_xyz, quat_unique + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from . import pose_commands_cfg as dex_cmd_cfgs + + +class ObjectUniformPoseCommand(CommandTerm): + """Uniform pose command generator for an object (in the robot base frame). + + This command term samples target object poses by: + • Drawing (x, y, z) uniformly within configured Cartesian bounds, and + • Drawing roll-pitch-yaw uniformly within configured ranges, then converting + to a quaternion (w, x, y, z). Optionally makes quaternions unique by enforcing + a positive real part. + + Frames: + Targets are defined in the robot's *base frame*. For metrics/visualization, + targets are transformed into the *world frame* using the robot root pose. + + Outputs: + The command buffer has shape (num_envs, 7): `(x, y, z, qw, qx, qy, qz)`. + + Metrics: + `position_error` and `orientation_error` are computed between the commanded + world-frame pose and the object's current world-frame pose. + + Config: + `cfg` must provide the sampling ranges, whether to enforce quaternion uniqueness, + and optional visualization settings. + """ + + cfg: dex_cmd_cfgs.ObjectUniformPoseCommandCfg + """Configuration for the command generator.""" + + def __init__(self, cfg: dex_cmd_cfgs.ObjectUniformPoseCommandCfg, env: ManagerBasedEnv): + """Initialize the command generator class. + + Args: + cfg: The configuration parameters for the command generator. + env: The environment object. + """ + # initialize the base class + super().__init__(cfg, env) + + # extract the robot and body index for which the command is generated + self.robot: Articulation = env.scene[cfg.asset_name] + self.object: RigidObject = env.scene[cfg.object_name] + self.success_vis_asset: RigidObject = env.scene[cfg.success_vis_asset_name] + + # create buffers + # -- commands: (x, y, z, qw, qx, qy, qz) in root frame + self.pose_command_b = torch.zeros(self.num_envs, 7, device=self.device) + self.pose_command_b[:, 3] = 1.0 + self.pose_command_w = torch.zeros_like(self.pose_command_b) + # -- metrics + self.metrics["position_error"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["orientation_error"] = torch.zeros(self.num_envs, device=self.device) + + self.success_visualizer = VisualizationMarkers(self.cfg.success_visualizer_cfg) + self.success_visualizer.set_visibility(True) + + def __str__(self) -> str: + msg = "UniformPoseCommand:\n" + msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" + msg += f"\tResampling time range: {self.cfg.resampling_time_range}\n" + return msg + + """ + Properties + """ + + @property + def command(self) -> torch.Tensor: + """The desired pose command. Shape is (num_envs, 7). + + The first three elements correspond to the position, followed by the quaternion orientation in (w, x, y, z). + """ + return self.pose_command_b + + """ + Implementation specific functions. + """ + + def _update_metrics(self): + # transform command from base frame to simulation world frame + self.pose_command_w[:, :3], self.pose_command_w[:, 3:] = combine_frame_transforms( + self.robot.data.root_pos_w, + self.robot.data.root_quat_w, + self.pose_command_b[:, :3], + self.pose_command_b[:, 3:], + ) + # compute the error + pos_error, rot_error = compute_pose_error( + self.pose_command_w[:, :3], + self.pose_command_w[:, 3:], + self.object.data.root_state_w[:, :3], + self.object.data.root_state_w[:, 3:7], + ) + self.metrics["position_error"] = torch.norm(pos_error, dim=-1) + self.metrics["orientation_error"] = torch.norm(rot_error, dim=-1) + + success_id = self.metrics["position_error"] < 0.05 + if not self.cfg.position_only: + success_id &= self.metrics["orientation_error"] < 0.5 + self.success_visualizer.visualize(self.success_vis_asset.data.root_pos_w, marker_indices=success_id.int()) + + def _resample_command(self, env_ids: Sequence[int]): + # sample new pose targets + # -- position + r = torch.empty(len(env_ids), device=self.device) + self.pose_command_b[env_ids, 0] = r.uniform_(*self.cfg.ranges.pos_x) + self.pose_command_b[env_ids, 1] = r.uniform_(*self.cfg.ranges.pos_y) + self.pose_command_b[env_ids, 2] = r.uniform_(*self.cfg.ranges.pos_z) + # -- orientation + euler_angles = torch.zeros_like(self.pose_command_b[env_ids, :3]) + euler_angles[:, 0].uniform_(*self.cfg.ranges.roll) + euler_angles[:, 1].uniform_(*self.cfg.ranges.pitch) + euler_angles[:, 2].uniform_(*self.cfg.ranges.yaw) + quat = quat_from_euler_xyz(euler_angles[:, 0], euler_angles[:, 1], euler_angles[:, 2]) + # make sure the quaternion has real part as positive + self.pose_command_b[env_ids, 3:] = quat_unique(quat) if self.cfg.make_quat_unique else quat + + def _update_command(self): + pass + + def _set_debug_vis_impl(self, debug_vis: bool): + # create markers if necessary for the first tome + if debug_vis: + if not hasattr(self, "goal_visualizer"): + # -- goal pose + self.goal_visualizer = VisualizationMarkers(self.cfg.goal_pose_visualizer_cfg) + # -- current body pose + self.curr_visualizer = VisualizationMarkers(self.cfg.curr_pose_visualizer_cfg) + # set their visibility to true + self.goal_visualizer.set_visibility(True) + self.curr_visualizer.set_visibility(True) + else: + if hasattr(self, "goal_visualizer"): + self.goal_visualizer.set_visibility(False) + self.curr_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + # check if robot is initialized + # note: this is needed in-case the robot is de-initialized. we can't access the data + if not self.robot.is_initialized: + return + # update the markers + if not self.cfg.position_only: + # -- goal pose + self.goal_visualizer.visualize(self.pose_command_w[:, :3], self.pose_command_w[:, 3:]) + # -- current object pose + self.curr_visualizer.visualize(self.object.data.root_pos_w, self.object.data.root_quat_w) + else: + distance = torch.norm(self.pose_command_w[:, :3] - self.object.data.root_pos_w[:, :3], dim=1) + success_id = (distance < 0.05).int() + # note: since marker indices for position is 1(far) and 2(near), we can simply shift the success_id by 1. + # -- goal position + self.goal_visualizer.visualize(self.pose_command_w[:, :3], marker_indices=success_id + 1) + # -- current object position + self.curr_visualizer.visualize(self.object.data.root_pos_w, marker_indices=success_id + 1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands_cfg.py new file mode 100644 index 00000000000..8501c00116d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/commands/pose_commands_cfg.py @@ -0,0 +1,92 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.managers import CommandTermCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import pose_commands as dex_cmd + +ALIGN_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "frame": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.1, 0.1, 0.1), + ), + "position_far": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + "position_near": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + } +) + + +@configclass +class ObjectUniformPoseCommandCfg(CommandTermCfg): + """Configuration for uniform pose command generator.""" + + class_type: type = dex_cmd.ObjectUniformPoseCommand + + asset_name: str = MISSING + """Name of the coordinate referencing asset in the environment for which the commands are generated respect to.""" + + object_name: str = MISSING + """Name of the object in the environment for which the commands are generated.""" + + make_quat_unique: bool = False + """Whether to make the quaternion unique or not. Defaults to False. + + If True, the quaternion is made unique by ensuring the real part is positive. + """ + + @configclass + class Ranges: + """Uniform distribution ranges for the pose commands.""" + + pos_x: tuple[float, float] = MISSING + """Range for the x position (in m).""" + + pos_y: tuple[float, float] = MISSING + """Range for the y position (in m).""" + + pos_z: tuple[float, float] = MISSING + """Range for the z position (in m).""" + + roll: tuple[float, float] = MISSING + """Range for the roll angle (in rad).""" + + pitch: tuple[float, float] = MISSING + """Range for the pitch angle (in rad).""" + + yaw: tuple[float, float] = MISSING + """Range for the yaw angle (in rad).""" + + ranges: Ranges = MISSING + """Ranges for the commands.""" + + position_only: bool = True + """Command goal position only. Command includes goal quat if False""" + + # Pose Markers + goal_pose_visualizer_cfg: VisualizationMarkersCfg = ALIGN_MARKER_CFG.replace(prim_path="/Visuals/Command/goal_pose") + """The configuration for the goal pose visualization marker. Defaults to FRAME_MARKER_CFG.""" + + curr_pose_visualizer_cfg: VisualizationMarkersCfg = ALIGN_MARKER_CFG.replace(prim_path="/Visuals/Command/body_pose") + """The configuration for the current pose visualization marker. Defaults to FRAME_MARKER_CFG.""" + + success_vis_asset_name: str = MISSING + """Name of the asset in the environment for which the success color are indicated.""" + + # success markers + success_visualizer_cfg = VisualizationMarkersCfg(prim_path="/Visuals/SuccessMarkers", markers={}) + """The configuration for the success visualization marker. User needs to add the markers""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/curriculums.py new file mode 100644 index 00000000000..c1a8c0f0d66 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/curriculums.py @@ -0,0 +1,113 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.envs import mdp +from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def initial_final_interpolate_fn(env: ManagerBasedRLEnv, env_id, data, initial_value, final_value, difficulty_term_str): + """ + Interpolate between initial value iv and final value fv, for any arbitrarily + nested structure of lists/tuples in 'data'. Scalars (int/float) are handled + at the leaves. + """ + # get the fraction scalar on the device + difficulty_term: DifficultyScheduler = getattr(env.curriculum_manager.cfg, difficulty_term_str).func + frac = difficulty_term.difficulty_frac + if frac < 0.1: + # no-op during start, since the difficulty fraction near 0 is wasting of resource. + return mdp.modify_env_param.NO_CHANGE + + # convert iv/fv to tensors, but we'll peel them apart in recursion + initial_value_tensor = torch.tensor(initial_value, device=env.device) + final_value_tensor = torch.tensor(final_value, device=env.device) + + return _recurse(initial_value_tensor.tolist(), final_value_tensor.tolist(), data, frac) + + +def _recurse(iv_elem, fv_elem, data_elem, frac): + # If it's a sequence, rebuild the same type with each element recursed + if isinstance(data_elem, Sequence) and not isinstance(data_elem, (str, bytes)): + # Note: we assume initial value element and final value element have the same structure as data + return type(data_elem)(_recurse(iv_e, fv_e, d_e, frac) for iv_e, fv_e, d_e in zip(iv_elem, fv_elem, data_elem)) + # Otherwise it's a leaf scalar: do the interpolation + new_val = frac * (fv_elem - iv_elem) + iv_elem + if isinstance(data_elem, int): + return int(new_val.item()) + else: + # cast floats or any numeric + return new_val.item() + + +class DifficultyScheduler(ManagerTermBase): + """Adaptive difficulty scheduler for curriculum learning. + + Tracks per-environment difficulty levels and adjusts them based on task performance. Difficulty increases when + position/orientation errors fall below given tolerances, and decreases otherwise (unless `promotion_only` is set). + The normalized average difficulty across environments is exposed as `difficulty_frac` for use in curriculum + interpolation. + + Args: + cfg: Configuration object specifying scheduler parameters. + env: The manager-based RL environment. + + """ + + def __init__(self, cfg, env): + super().__init__(cfg, env) + init_difficulty = self.cfg.params.get("init_difficulty", 0) + self.current_adr_difficulties = torch.ones(env.num_envs, device=env.device) * init_difficulty + self.difficulty_frac = 0 + + def get_state(self): + return self.current_adr_difficulties + + def set_state(self, state: torch.Tensor): + self.current_adr_difficulties = state.clone().to(self._env.device) + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + pos_tol: float = 0.1, + rot_tol: float | None = None, + init_difficulty: int = 0, + min_difficulty: int = 0, + max_difficulty: int = 50, + promotion_only: bool = False, + ): + asset: Articulation = env.scene[asset_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + command = env.command_manager.get_command("object_pose") + des_pos_w, des_quat_w = combine_frame_transforms( + asset.data.root_pos_w[env_ids], asset.data.root_quat_w[env_ids], command[env_ids, :3], command[env_ids, 3:7] + ) + pos_err, rot_err = compute_pose_error( + des_pos_w, des_quat_w, object.data.root_pos_w[env_ids], object.data.root_quat_w[env_ids] + ) + pos_dist = torch.norm(pos_err, dim=1) + rot_dist = torch.norm(rot_err, dim=1) + move_up = (pos_dist < pos_tol) & (rot_dist < rot_tol) if rot_tol else pos_dist < pos_tol + demot = self.current_adr_difficulties[env_ids] if promotion_only else self.current_adr_difficulties[env_ids] - 1 + self.current_adr_difficulties[env_ids] = torch.where( + move_up, + self.current_adr_difficulties[env_ids] + 1, + demot, + ).clamp(min=min_difficulty, max=max_difficulty) + self.difficulty_frac = torch.mean(self.current_adr_difficulties) / max(max_difficulty, 1) + return self.difficulty_frac diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/observations.py new file mode 100644 index 00000000000..b48e4bcfb5c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/observations.py @@ -0,0 +1,197 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.utils.math import quat_apply, quat_apply_inverse, quat_inv, quat_mul, subtract_frame_transforms + +from .utils import sample_object_point_cloud + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_pos_b( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +): + """Object position in the robot's root frame. + + Args: + env: The environment. + robot_cfg: Scene entity for the robot (reference frame). Defaults to ``SceneEntityCfg("robot")``. + object_cfg: Scene entity for the object. Defaults to ``SceneEntityCfg("object")``. + + Returns: + Tensor of shape ``(num_envs, 3)``: object position [x, y, z] expressed in the robot root frame. + """ + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + return quat_apply_inverse(robot.data.root_quat_w, object.data.root_pos_w - robot.data.root_pos_w) + + +def object_quat_b( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Object orientation in the robot's root frame. + + Args: + env: The environment. + robot_cfg: Scene entity for the robot (reference frame). Defaults to ``SceneEntityCfg("robot")``. + object_cfg: Scene entity for the object. Defaults to ``SceneEntityCfg("object")``. + + Returns: + Tensor of shape ``(num_envs, 4)``: object quaternion ``(w, x, y, z)`` in the robot root frame. + """ + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + return quat_mul(quat_inv(robot.data.root_quat_w), object.data.root_quat_w) + + +def body_state_b( + env: ManagerBasedRLEnv, + body_asset_cfg: SceneEntityCfg, + base_asset_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Body state (pos, quat, lin vel, ang vel) in the base asset's root frame. + + The state for each body is stacked horizontally as + ``[position(3), quaternion(4)(wxyz), linvel(3), angvel(3)]`` and then concatenated over bodies. + + Args: + env: The environment. + body_asset_cfg: Scene entity for the articulated body whose links are observed. + base_asset_cfg: Scene entity providing the reference (root) frame. + + Returns: + Tensor of shape ``(num_envs, num_bodies * 13)`` with per-body states expressed in the base root frame. + """ + body_asset: Articulation = env.scene[body_asset_cfg.name] + base_asset: Articulation = env.scene[base_asset_cfg.name] + # get world pose of bodies + body_pos_w = body_asset.data.body_pos_w[:, body_asset_cfg.body_ids].view(-1, 3) + body_quat_w = body_asset.data.body_quat_w[:, body_asset_cfg.body_ids].view(-1, 4) + body_lin_vel_w = body_asset.data.body_lin_vel_w[:, body_asset_cfg.body_ids].view(-1, 3) + body_ang_vel_w = body_asset.data.body_ang_vel_w[:, body_asset_cfg.body_ids].view(-1, 3) + num_bodies = int(body_pos_w.shape[0] / env.num_envs) + # get world pose of base frame + root_pos_w = base_asset.data.root_link_pos_w.unsqueeze(1).repeat_interleave(num_bodies, dim=1).view(-1, 3) + root_quat_w = base_asset.data.root_link_quat_w.unsqueeze(1).repeat_interleave(num_bodies, dim=1).view(-1, 4) + # transform from world body pose to local body pose + body_pos_b, body_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, body_pos_w, body_quat_w) + body_lin_vel_b = quat_apply_inverse(root_quat_w, body_lin_vel_w) + body_ang_vel_b = quat_apply_inverse(root_quat_w, body_ang_vel_w) + # concate and return + out = torch.cat((body_pos_b, body_quat_b, body_lin_vel_b, body_ang_vel_b), dim=1) + return out.view(env.num_envs, -1) + + +class object_point_cloud_b(ManagerTermBase): + """Object surface point cloud expressed in a reference asset's root frame. + + Points are pre-sampled on the object's surface in its local frame and transformed to world, + then into the reference (e.g., robot) root frame. Optionally visualizes the points. + + Args (from ``cfg.params``): + object_cfg: Scene entity for the object to sample. Defaults to ``SceneEntityCfg("object")``. + ref_asset_cfg: Scene entity providing the reference frame. Defaults to ``SceneEntityCfg("robot")``. + num_points: Number of points to sample on the object surface. Defaults to ``10``. + visualize: Whether to draw markers for the points. Defaults to ``True``. + static: If ``True``, cache world-space points on reset and reuse them (no per-step resampling). + + Returns (from ``__call__``): + If ``flatten=False``: tensor of shape ``(num_envs, num_points, 3)``. + If ``flatten=True``: tensor of shape ``(num_envs, 3 * num_points)``. + """ + + def __init__(self, cfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + + self.object_cfg: SceneEntityCfg = cfg.params.get("object_cfg", SceneEntityCfg("object")) + self.ref_asset_cfg: SceneEntityCfg = cfg.params.get("ref_asset_cfg", SceneEntityCfg("robot")) + num_points: int = cfg.params.get("num_points", 10) + self.object: RigidObject = env.scene[self.object_cfg.name] + self.ref_asset: Articulation = env.scene[self.ref_asset_cfg.name] + # lazy initialize visualizer and point cloud + if cfg.params.get("visualize", True): + from isaaclab.markers import VisualizationMarkers + from isaaclab.markers.config import RAY_CASTER_MARKER_CFG + + ray_cfg = RAY_CASTER_MARKER_CFG.replace(prim_path="/Visuals/ObservationPointCloud") + ray_cfg.markers["hit"].radius = 0.0025 + self.visualizer = VisualizationMarkers(ray_cfg) + self.points_local = sample_object_point_cloud( + env.num_envs, num_points, self.object.cfg.prim_path, device=env.device + ) + self.points_w = torch.zeros_like(self.points_local) + + def __call__( + self, + env: ManagerBasedRLEnv, + ref_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + num_points: int = 10, + flatten: bool = False, + visualize: bool = True, + ): + """Compute the object point cloud in the reference asset's root frame. + + Note: + Points are pre-sampled at initialization using ``self.num_points``; the ``num_points`` argument is + kept for API symmetry and does not change the sampled set at runtime. + + Args: + env: The environment. + ref_asset_cfg: Reference frame provider (root). Defaults to ``SceneEntityCfg("robot")``. + object_cfg: Object to sample. Defaults to ``SceneEntityCfg("object")``. + num_points: Unused at runtime; see note above. + flatten: If ``True``, return a flattened tensor ``(num_envs, 3 * num_points)``. + visualize: If ``True``, draw markers for the points. + + Returns: + Tensor of shape ``(num_envs, num_points, 3)`` or flattened if requested. + """ + ref_pos_w = self.ref_asset.data.root_pos_w.unsqueeze(1).repeat(1, num_points, 1) + ref_quat_w = self.ref_asset.data.root_quat_w.unsqueeze(1).repeat(1, num_points, 1) + + object_pos_w = self.object.data.root_pos_w.unsqueeze(1).repeat(1, num_points, 1) + object_quat_w = self.object.data.root_quat_w.unsqueeze(1).repeat(1, num_points, 1) + # apply rotation + translation + self.points_w = quat_apply(object_quat_w, self.points_local) + object_pos_w + if visualize: + self.visualizer.visualize(translations=self.points_w.view(-1, 3)) + object_point_cloud_pos_b, _ = subtract_frame_transforms(ref_pos_w, ref_quat_w, self.points_w, None) + + return object_point_cloud_pos_b.view(env.num_envs, -1) if flatten else object_point_cloud_pos_b + + +def fingers_contact_force_b( + env: ManagerBasedRLEnv, + contact_sensor_names: list[str], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """base-frame contact forces from listed sensors, concatenated per env. + + Args: + env: The environment. + contact_sensor_names: Names of contact sensors in ``env.scene.sensors`` to read. + + Returns: + Tensor of shape ``(num_envs, 3 * num_sensors)`` with forces stacked horizontally as + ``[fx, fy, fz]`` per sensor. + """ + force_w = [env.scene.sensors[name].data.force_matrix_w.view(env.num_envs, 3) for name in contact_sensor_names] + force_w = torch.stack(force_w, dim=1) + robot: Articulation = env.scene[asset_cfg.name] + forces_b = quat_apply_inverse(robot.data.root_link_quat_w.unsqueeze(1).repeat(1, force_w.shape[1], 1), force_w) + return forces_b diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py new file mode 100644 index 00000000000..9a6170f1e4f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py @@ -0,0 +1,126 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensor +from isaaclab.utils import math as math_utils +from isaaclab.utils.math import combine_frame_transforms, compute_pose_error + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def action_rate_l2_clamped(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize the rate of change of the actions using L2 squared kernel.""" + return torch.sum(torch.square(env.action_manager.action - env.action_manager.prev_action), dim=1).clamp(-1000, 1000) + + +def action_l2_clamped(env: ManagerBasedRLEnv) -> torch.Tensor: + """Penalize the actions using L2 squared kernel.""" + return torch.sum(torch.square(env.action_manager.action), dim=1).clamp(-1000, 1000) + + +def object_ee_distance( + env: ManagerBasedRLEnv, + std: float, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Reward reaching the object using a tanh-kernel on end-effector distance. + + The reward is close to 1 when the maximum distance between the object and any end-effector body is small. + """ + asset: RigidObject = env.scene[asset_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + asset_pos = asset.data.body_pos_w[:, asset_cfg.body_ids] + object_pos = object.data.root_pos_w + object_ee_distance = torch.norm(asset_pos - object_pos[:, None, :], dim=-1).max(dim=-1).values + return 1 - torch.tanh(object_ee_distance / std) + + +def contacts(env: ManagerBasedRLEnv, threshold: float) -> torch.Tensor: + """Penalize undesired contacts as the number of violations that are above a threshold.""" + + thumb_contact_sensor: ContactSensor = env.scene.sensors["thumb_link_3_object_s"] + index_contact_sensor: ContactSensor = env.scene.sensors["index_link_3_object_s"] + middle_contact_sensor: ContactSensor = env.scene.sensors["middle_link_3_object_s"] + ring_contact_sensor: ContactSensor = env.scene.sensors["ring_link_3_object_s"] + # check if contact force is above threshold + thumb_contact = thumb_contact_sensor.data.force_matrix_w.view(env.num_envs, 3) + index_contact = index_contact_sensor.data.force_matrix_w.view(env.num_envs, 3) + middle_contact = middle_contact_sensor.data.force_matrix_w.view(env.num_envs, 3) + ring_contact = ring_contact_sensor.data.force_matrix_w.view(env.num_envs, 3) + + thumb_contact_mag = torch.norm(thumb_contact, dim=-1) + index_contact_mag = torch.norm(index_contact, dim=-1) + middle_contact_mag = torch.norm(middle_contact, dim=-1) + ring_contact_mag = torch.norm(ring_contact, dim=-1) + good_contact_cond1 = (thumb_contact_mag > threshold) & ( + (index_contact_mag > threshold) | (middle_contact_mag > threshold) | (ring_contact_mag > threshold) + ) + + return good_contact_cond1 + + +def success_reward( + env: ManagerBasedRLEnv, + command_name: str, + asset_cfg: SceneEntityCfg, + align_asset_cfg: SceneEntityCfg, + pos_std: float, + rot_std: float | None = None, +) -> torch.Tensor: + """Reward success by comparing commanded pose to the object pose using tanh kernels on error.""" + + asset: RigidObject = env.scene[asset_cfg.name] + object: RigidObject = env.scene[align_asset_cfg.name] + command = env.command_manager.get_command(command_name) + des_pos_w, des_quat_w = combine_frame_transforms( + asset.data.root_pos_w, asset.data.root_quat_w, command[:, :3], command[:, 3:7] + ) + pos_err, rot_err = compute_pose_error(des_pos_w, des_quat_w, object.data.root_pos_w, object.data.root_quat_w) + pos_dist = torch.norm(pos_err, dim=1) + if not rot_std: + # square is not necessary but this help to keep the final value between having rot_std or not roughly the same + return (1 - torch.tanh(pos_dist / pos_std)) ** 2 + rot_dist = torch.norm(rot_err, dim=1) + return (1 - torch.tanh(pos_dist / pos_std)) * (1 - torch.tanh(rot_dist / rot_std)) + + +def position_command_error_tanh( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg, align_asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward tracking of commanded position using tanh kernel, gated by contact presence.""" + + asset: RigidObject = env.scene[asset_cfg.name] + object: RigidObject = env.scene[align_asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms(asset.data.root_pos_w, asset.data.root_quat_w, des_pos_b) + distance = torch.norm(object.data.root_pos_w - des_pos_w, dim=1) + return (1 - torch.tanh(distance / std)) * contacts(env, 1.0).float() + + +def orientation_command_error_tanh( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg, align_asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward tracking of commanded orientation using tanh kernel, gated by contact presence.""" + + asset: RigidObject = env.scene[asset_cfg.name] + object: RigidObject = env.scene[align_asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current orientations + des_quat_b = command[:, 3:7] + des_quat_w = math_utils.quat_mul(asset.data.root_state_w[:, 3:7], des_quat_b) + quat_distance = math_utils.quat_error_magnitude(object.data.root_quat_w, des_quat_w) + + return (1 - torch.tanh(quat_distance / std)) * contacts(env, 1.0).float() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/terminations.py new file mode 100644 index 00000000000..3ef9cf14b0a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/terminations.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the dexsuite task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def out_of_bound( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("object"), + in_bound_range: dict[str, tuple[float, float]] = {}, +) -> torch.Tensor: + """Termination condition for the object falls out of bound. + + Args: + env: The environment. + asset_cfg: The object configuration. Defaults to SceneEntityCfg("object"). + in_bound_range: The range in x, y, z such that the object is considered in range + """ + object: RigidObject = env.scene[asset_cfg.name] + range_list = [in_bound_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=env.device) + + object_pos_local = object.data.root_pos_w - env.scene.env_origins + outside_bounds = ((object_pos_local < ranges[:, 0]) | (object_pos_local > ranges[:, 1])).any(dim=1) + return outside_bounds + + +def abnormal_robot_state(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Terminating environment when violation of velocity limits detects, this usually indicates unstable physics caused + by very bad, or aggressive action""" + robot: Articulation = env.scene[asset_cfg.name] + return (robot.data.joint_vel.abs() > (robot.data.joint_vel_limits * 2)).any(dim=1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py new file mode 100644 index 00000000000..f7b8e9db59b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py @@ -0,0 +1,247 @@ +# Copyright (c) 2022-2025, 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 hashlib +import logging +import numpy as np +import torch +import trimesh +from trimesh.sample import sample_surface + +import isaacsim.core.utils.prims as prim_utils +from pxr import UsdGeom + +from isaaclab.sim.utils import get_all_matching_child_prims + +# ---- module-scope caches ---- +_PRIM_SAMPLE_CACHE: dict[tuple[str, int], np.ndarray] = {} # (prim_hash, num_points) -> (N,3) in root frame +_FINAL_SAMPLE_CACHE: dict[str, np.ndarray] = {} # env_hash -> (num_points,3) in root frame + + +def clear_pointcloud_caches(): + _PRIM_SAMPLE_CACHE.clear() + _FINAL_SAMPLE_CACHE.clear() + + +def sample_object_point_cloud(num_envs: int, num_points: int, prim_path: str, device: str = "cpu") -> torch.Tensor: + """ + Samples point clouds for each environment instance by collecting points + from all matching USD prims under `prim_path`, then downsamples to + exactly `num_points` per env using farthest-point sampling. + + Caching is in-memory within this module: + - per-prim raw samples: _PRIM_SAMPLE_CACHE[(prim_hash, num_points)] + - final downsampled env: _FINAL_SAMPLE_CACHE[env_hash] + + Returns: + torch.Tensor: Shape (num_envs, num_points, 3) on `device`. + """ + points = torch.zeros((num_envs, num_points, 3), dtype=torch.float32, device=device) + xform_cache = UsdGeom.XformCache() + + for i in range(num_envs): + # Resolve prim path + obj_path = prim_path.replace(".*", str(i)) + + # Gather prims + prims = get_all_matching_child_prims( + obj_path, predicate=lambda p: p.GetTypeName() in ("Mesh", "Cube", "Sphere", "Cylinder", "Capsule", "Cone") + ) + if not prims: + raise KeyError(f"No valid prims under {obj_path}") + + object_prim = prim_utils.get_prim_at_path(obj_path) + world_root = xform_cache.GetLocalToWorldTransform(object_prim) + + # hash each child prim by its rel transform + geometry + prim_hashes = [] + for prim in prims: + prim_type = prim.GetTypeName() + hasher = hashlib.sha256() + + rel = world_root.GetInverse() * xform_cache.GetLocalToWorldTransform(prim) # prim -> root + mat_np = np.array([[rel[r][c] for c in range(4)] for r in range(4)], dtype=np.float32) + hasher.update(mat_np.tobytes()) + + if prim_type == "Mesh": + mesh = UsdGeom.Mesh(prim) + verts = np.asarray(mesh.GetPointsAttr().Get(), dtype=np.float32) + hasher.update(verts.tobytes()) + else: + if prim_type == "Cube": + size = UsdGeom.Cube(prim).GetSizeAttr().Get() + hasher.update(np.float32(size).tobytes()) + elif prim_type == "Sphere": + r = UsdGeom.Sphere(prim).GetRadiusAttr().Get() + hasher.update(np.float32(r).tobytes()) + elif prim_type == "Cylinder": + c = UsdGeom.Cylinder(prim) + hasher.update(np.float32(c.GetRadiusAttr().Get()).tobytes()) + hasher.update(np.float32(c.GetHeightAttr().Get()).tobytes()) + elif prim_type == "Capsule": + c = UsdGeom.Capsule(prim) + hasher.update(np.float32(c.GetRadiusAttr().Get()).tobytes()) + hasher.update(np.float32(c.GetHeightAttr().Get()).tobytes()) + elif prim_type == "Cone": + c = UsdGeom.Cone(prim) + hasher.update(np.float32(c.GetRadiusAttr().Get()).tobytes()) + hasher.update(np.float32(c.GetHeightAttr().Get()).tobytes()) + + prim_hashes.append(hasher.hexdigest()) + + # scale on root (default to 1 if missing) + attr = object_prim.GetAttribute("xformOp:scale") + scale_val = attr.Get() if attr else None + if scale_val is None: + base_scale = torch.ones(3, dtype=torch.float32, device=device) + else: + base_scale = torch.tensor(scale_val, dtype=torch.float32, device=device) + + # env-level cache key (includes num_points) + env_key = "_".join(sorted(prim_hashes)) + f"_{num_points}" + env_hash = hashlib.sha256(env_key.encode()).hexdigest() + + # load from env-level in-memory cache + if env_hash in _FINAL_SAMPLE_CACHE: + arr = _FINAL_SAMPLE_CACHE[env_hash] # (num_points,3) in root frame + points[i] = torch.from_numpy(arr).to(device) * base_scale.unsqueeze(0) + continue + + # otherwise build per-prim samples (with per-prim cache) + all_samples_np: list[np.ndarray] = [] + for prim, ph in zip(prims, prim_hashes): + key = (ph, num_points) + if key in _PRIM_SAMPLE_CACHE: + samples = _PRIM_SAMPLE_CACHE[key] + else: + prim_type = prim.GetTypeName() + if prim_type == "Mesh": + mesh = UsdGeom.Mesh(prim) + verts = np.asarray(mesh.GetPointsAttr().Get(), dtype=np.float32) + faces = _triangulate_faces(prim) + mesh_tm = trimesh.Trimesh(vertices=verts, faces=faces, process=False) + else: + mesh_tm = create_primitive_mesh(prim) + + face_weights = mesh_tm.area_faces + samples_np, _ = sample_surface(mesh_tm, num_points * 2, face_weight=face_weights) + + # FPS to num_points on chosen device + tensor_pts = torch.from_numpy(samples_np.astype(np.float32)).to(device) + prim_idxs = farthest_point_sampling(tensor_pts, num_points) + local_pts = tensor_pts[prim_idxs] + + # prim -> root transform + rel = xform_cache.GetLocalToWorldTransform(prim) * world_root.GetInverse() + mat_np = np.array([[rel[r][c] for c in range(4)] for r in range(4)], dtype=np.float32) + mat_t = torch.from_numpy(mat_np).to(device) + + ones = torch.ones((num_points, 1), device=device) + pts_h = torch.cat([local_pts, ones], dim=1) + root_h = pts_h @ mat_t + samples = root_h[:, :3].detach().cpu().numpy() + + if prim_type == "Cone": + samples[:, 2] -= UsdGeom.Cone(prim).GetHeightAttr().Get() / 2 + + _PRIM_SAMPLE_CACHE[key] = samples # cache in root frame @ num_points + + all_samples_np.append(samples) + + # combine & env-level FPS (if needed) + if len(all_samples_np) == 1: + samples_final = torch.from_numpy(all_samples_np[0]).to(device) + else: + combined = torch.from_numpy(np.concatenate(all_samples_np, axis=0)).to(device) + idxs = farthest_point_sampling(combined, num_points) + samples_final = combined[idxs] + + # store env-level cache in root frame (CPU) + _FINAL_SAMPLE_CACHE[env_hash] = samples_final.detach().cpu().numpy() + + # apply root scale and write out + points[i] = samples_final * base_scale.unsqueeze(0) + + return points + + +def _triangulate_faces(prim) -> np.ndarray: + """Convert a USD Mesh prim into triangulated face indices (N, 3).""" + mesh = UsdGeom.Mesh(prim) + counts = mesh.GetFaceVertexCountsAttr().Get() + indices = mesh.GetFaceVertexIndicesAttr().Get() + faces = [] + it = iter(indices) + for cnt in counts: + poly = [next(it) for _ in range(cnt)] + for k in range(1, cnt - 1): + faces.append([poly[0], poly[k], poly[k + 1]]) + return np.asarray(faces, dtype=np.int64) + + +def create_primitive_mesh(prim) -> trimesh.Trimesh: + """Create a trimesh mesh from a USD primitive (Cube, Sphere, Cylinder, etc.).""" + prim_type = prim.GetTypeName() + if prim_type == "Cube": + size = UsdGeom.Cube(prim).GetSizeAttr().Get() + return trimesh.creation.box(extents=(size, size, size)) + elif prim_type == "Sphere": + r = UsdGeom.Sphere(prim).GetRadiusAttr().Get() + return trimesh.creation.icosphere(subdivisions=3, radius=r) + elif prim_type == "Cylinder": + c = UsdGeom.Cylinder(prim) + return trimesh.creation.cylinder(radius=c.GetRadiusAttr().Get(), height=c.GetHeightAttr().Get()) + elif prim_type == "Capsule": + c = UsdGeom.Capsule(prim) + return trimesh.creation.capsule(radius=c.GetRadiusAttr().Get(), height=c.GetHeightAttr().Get()) + elif prim_type == "Cone": # Cone + c = UsdGeom.Cone(prim) + return trimesh.creation.cone(radius=c.GetRadiusAttr().Get(), height=c.GetHeightAttr().Get()) + else: + raise KeyError(f"{prim_type} is not a valid primitive mesh type") + + +def farthest_point_sampling( + points: torch.Tensor, n_samples: int, memory_threashold=2 * 1024**3 +) -> torch.Tensor: # 2 GiB + """ + Farthest Point Sampling (FPS) for point sets. + + Selects `n_samples` points such that each new point is farthest from the + already chosen ones. Uses a full pairwise distance matrix if memory allows, + otherwise falls back to an iterative version. + + Args: + points (torch.Tensor): Input points of shape (N, D). + n_samples (int): Number of samples to select. + memory_threashold (int): Max allowed bytes for distance matrix. Default 2 GiB. + + Returns: + torch.Tensor: Indices of sampled points (n_samples,). + """ + device = points.device + N = points.shape[0] + elem_size = points.element_size() + bytes_needed = N * N * elem_size + if bytes_needed <= memory_threashold: + dist_mat = torch.cdist(points, points) + sampled_idx = torch.zeros(n_samples, dtype=torch.long, device=device) + min_dists = torch.full((N,), float("inf"), device=device) + farthest = torch.randint(0, N, (1,), device=device) + for j in range(n_samples): + sampled_idx[j] = farthest + min_dists = torch.minimum(min_dists, dist_mat[farthest].view(-1)) + farthest = torch.argmax(min_dists) + return sampled_idx + logging.warning(f"FPS fallback to iterative (needed {bytes_needed} > {memory_threashold})") + sampled_idx = torch.zeros(n_samples, dtype=torch.long, device=device) + distances = torch.full((N,), float("inf"), device=device) + farthest = torch.randint(0, N, (1,), device=device) + for j in range(n_samples): + sampled_idx[j] = farthest + dist = torch.norm(points - points[farthest], dim=1) + distances = torch.minimum(distances, dist) + farthest = torch.argmax(distances) + return sampled_idx diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml index 1537f0d4c44..6e12c4940fa 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [512, 256, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml index 6d5d34de5a3..5ddcf1713e7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [256, 128, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py index ff72798e0f4..740a487b2a5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py @@ -12,6 +12,7 @@ nutpour_gr1t2_pink_ik_env_cfg, pickplace_gr1t2_env_cfg, pickplace_gr1t2_waist_enabled_env_cfg, + pickplace_unitree_g1_inspire_hand_env_cfg, ) gym.register( @@ -53,3 +54,13 @@ }, disable_env_checker=True, ) + +gym.register( + id="Isaac-PickPlace-G1-InspireFTP-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": pickplace_unitree_g1_inspire_hand_env_cfg.PickPlaceG1InspireFTPEnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index ed1f0f06130..2d7a69653fa 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py @@ -184,13 +184,16 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("robot")}, ) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) - - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) robot_pov_cam = ObsTerm( func=mdp.image, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index 0a3cb26b4d3..01feeab1cc2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -42,49 +42,6 @@ def __post_init__(self): "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -164,14 +121,7 @@ def __post_init__(self): ], ), ], - fixed_input_tasks=[ - # COMMENT OUT IF LOCKING WAIST/HEAD - # FrameTask( - # "GR1T2_fourier_hand_6dof_head_yaw_link", - # position_cost=1.0, # [cost] / [m] - # orientation_cost=0.05, # [cost] / [rad] - # ), - ], + fixed_input_tasks=[], xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), ), ) @@ -179,9 +129,6 @@ def __post_init__(self): temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True ) - ControllerUtils.change_revolute_to_fixed( - temp_urdf_output_path, self.actions.gr1_action.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py index efc8d9f7b1e..b4dfcb6829f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py @@ -14,6 +14,8 @@ def object_obs( env: ManagerBasedRLEnv, + left_eef_link_name: str, + right_eef_link_name: str, ) -> torch.Tensor: """ Object observations (in world frame): @@ -24,8 +26,8 @@ def object_obs( """ body_pos_w = env.scene["robot"].data.body_pos_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(left_eef_link_name) + right_eef_idx = env.scene["robot"].data.body_names.index(right_eef_link_name) left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins @@ -46,63 +48,32 @@ def object_obs( ) -def get_left_eef_pos( - env: ManagerBasedRLEnv, -) -> torch.Tensor: +def get_eef_pos(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: body_pos_w = env.scene["robot"].data.body_pos_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins return left_eef_pos -def get_left_eef_quat( - env: ManagerBasedRLEnv, -) -> torch.Tensor: +def get_eef_quat(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: body_quat_w = env.scene["robot"].data.body_quat_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) left_eef_quat = body_quat_w[:, left_eef_idx] return left_eef_quat -def get_right_eef_pos( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - body_pos_w = env.scene["robot"].data.body_pos_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") - right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins - - return right_eef_pos - - -def get_right_eef_quat( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - body_quat_w = env.scene["robot"].data.body_quat_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") - right_eef_quat = body_quat_w[:, right_eef_idx] - - return right_eef_quat - - -def get_hand_state( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - hand_joint_states = env.scene["robot"].data.joint_pos[:, -22:] # Hand joints are last 22 entries of joint state - - return hand_joint_states - - -def get_head_state( +def get_robot_joint_state( env: ManagerBasedRLEnv, + joint_names: list[str], ) -> torch.Tensor: - robot_joint_names = env.scene["robot"].data.joint_names - head_joint_names = ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"] - indexes = torch.tensor([robot_joint_names.index(name) for name in head_joint_names], dtype=torch.long) - head_joint_states = env.scene["robot"].data.joint_pos[:, indexes] + # hand_joint_names is a list of regex, use find_joints + indexes, _ = env.scene["robot"].find_joints(joint_names) + indexes = torch.tensor(indexes, dtype=torch.long) + robot_joint_states = env.scene["robot"].data.joint_pos[:, indexes] - return head_joint_states + return robot_joint_states def get_all_robot_link_state( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py index ee6dbd68526..477552bbdba 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py @@ -23,6 +23,7 @@ def task_done_pick_place( env: ManagerBasedRLEnv, + task_link_name: str = "", object_cfg: SceneEntityCfg = SceneEntityCfg("object"), right_wrist_max_x: float = 0.26, min_x: float = 0.40, @@ -54,6 +55,9 @@ def task_done_pick_place( Returns: Boolean tensor indicating which environments have completed the task. """ + if task_link_name == "": + raise ValueError("task_link_name must be provided to task_done_pick_place") + # Get object entity from the scene object: RigidObject = env.scene[object_cfg.name] @@ -65,7 +69,7 @@ def task_done_pick_place( # Get right wrist position relative to environment origin robot_body_pos_w = env.scene["robot"].data.body_pos_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + right_eef_idx = env.scene["robot"].data.body_names.index(task_link_name) right_wrist_x = robot_body_pos_w[:, right_eef_idx, 0] - env.scene.env_origins[:, 0] # Check all success conditions and combine with logical AND diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index ffa7929c953..6aaf5defb38 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py @@ -205,13 +205,16 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("robot")}, ) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) - - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) robot_pov_cam = ObsTerm( func=mdp.image, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index b7e1ff3ddec..6dcdd9a1e8f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -40,49 +40,6 @@ def __post_init__(self): "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -162,14 +119,7 @@ def __post_init__(self): ], ), ], - fixed_input_tasks=[ - # COMMENT OUT IF LOCKING WAIST/HEAD - # FrameTask( - # "GR1T2_fourier_hand_6dof_head_yaw_link", - # position_cost=1.0, # [cost] / [m] - # orientation_cost=0.05, # [cost] / [rad] - # ), - ], + fixed_input_tasks=[], xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), ), ) @@ -177,9 +127,6 @@ def __post_init__(self): temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True ) - ControllerUtils.change_revolute_to_fixed( - temp_urdf_output_path, self.actions.gr1_action.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index 9343db5ffc5..4b073b35a3f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -116,7 +116,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): class ActionsCfg: """Action specifications for the MDP.""" - pink_ik_cfg = PinkInverseKinematicsActionCfg( + upper_body_ik = PinkInverseKinematicsActionCfg( pink_controlled_joint_names=[ "left_shoulder_pitch_joint", "left_shoulder_roll_joint", @@ -133,49 +133,6 @@ class ActionsCfg: "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -220,14 +177,14 @@ class ActionsCfg: "GR1T2_fourier_hand_6dof_left_hand_pitch_link", position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] - lm_damping=10, # dampening for solver for step jumps + lm_damping=12, # dampening for solver for step jumps gain=0.5, ), FrameTask( "GR1T2_fourier_hand_6dof_right_hand_pitch_link", position_cost=8.0, # [cost] / [m] orientation_cost=1.0, # [cost] / [rad] - lm_damping=10, # dampening for solver for step jumps + lm_damping=12, # dampening for solver for step jumps gain=0.5, ), DampingTask( @@ -280,15 +237,21 @@ class PolicyCfg(ObsGroup): object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) - object = ObsTerm(func=mdp.object_obs) + object = ObsTerm( + func=mdp.object_obs, + params={"left_eef_link_name": "left_hand_roll_link", "right_eef_link_name": "right_hand_roll_link"}, + ) def __post_init__(self): self.enable_corruption = False @@ -308,7 +271,7 @@ class TerminationsCfg: func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} ) - success = DoneTerm(func=mdp.task_done_pick_place) + success = DoneTerm(func=mdp.task_done_pick_place, params={"task_link_name": "right_hand_roll_link"}) @configclass @@ -416,13 +379,10 @@ def __post_init__(self): temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True ) - ControllerUtils.change_revolute_to_fixed( - temp_urdf_output_path, self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller - self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path - self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path self.teleop_devices = DevicesCfg( devices={ @@ -433,7 +393,7 @@ def __post_init__(self): # number of joints in both hands num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, - hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, @@ -445,7 +405,7 @@ def __post_init__(self): enable_visualization=True, num_open_xr_hand_joints=2 * 26, sim_device=self.sim.device, - hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py index 636f347109f..30b17e89493 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -57,20 +57,16 @@ def __post_init__(self): # Add waist joint to pink_ik_cfg waist_joint_names = ["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"] for joint_name in waist_joint_names: - self.actions.pink_ik_cfg.pink_controlled_joint_names.append(joint_name) - self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove(joint_name) + self.actions.upper_body_ik.pink_controlled_joint_names.append(joint_name) # Convert USD to URDF and change revolute joints to fixed temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True ) - ControllerUtils.change_revolute_to_fixed( - temp_urdf_output_path, self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller - self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path - self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path self.teleop_devices = DevicesCfg( devices={ @@ -81,7 +77,7 @@ def __post_init__(self): # number of joints in both hands num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, - hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py new file mode 100644 index 00000000000..a557911498a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_unitree_g1_inspire_hand_env_cfg.py @@ -0,0 +1,409 @@ +# Copyright (c) 2022-2025, 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 tempfile +import torch + +import carb +from pink.tasks import FrameTask + +import isaaclab.controllers.utils as ControllerUtils +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import ManusViveCfg, OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.inspire.g1_upper_body_retargeter import UnitreeG1RetargeterCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +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 SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from . import mdp + +from isaaclab_assets.robots.unitree import G1_INSPIRE_FTP_CFG # isort: skip + + +## +# Scene definition +## +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.9996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=MassPropertiesCfg( + mass=0.05, + ), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = G1_INSPIRE_FTP_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 1.0), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + # right-arm + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + # left-arm + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + # -- + "waist_.*": 0.0, + ".*_hip_.*": 0.0, + ".*_knee_.*": 0.0, + ".*_ankle_.*": 0.0, + # -- left/right hand + ".*_thumb_.*": 0.0, + ".*_index_.*": 0.0, + ".*_middle_.*": 0.0, + ".*_ring_.*": 0.0, + ".*_pinky_.*": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + pink_ik_cfg = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_yaw_joint", + ".*_wrist_roll_joint", + ".*_wrist_pitch_joint", + ], + hand_joint_names=[ + # All the drive and mimic joints, total 24 joints + "L_index_proximal_joint", + "L_middle_proximal_joint", + "L_pinky_proximal_joint", + "L_ring_proximal_joint", + "L_thumb_proximal_yaw_joint", + "R_index_proximal_joint", + "R_middle_proximal_joint", + "R_pinky_proximal_joint", + "R_ring_proximal_joint", + "R_thumb_proximal_yaw_joint", + "L_index_intermediate_joint", + "L_middle_intermediate_joint", + "L_pinky_intermediate_joint", + "L_ring_intermediate_joint", + "L_thumb_proximal_pitch_joint", + "R_index_intermediate_joint", + "R_middle_intermediate_joint", + "R_pinky_intermediate_joint", + "R_ring_intermediate_joint", + "R_thumb_proximal_pitch_joint", + "L_thumb_intermediate_joint", + "R_thumb_intermediate_joint", + "L_thumb_distal_joint", + "R_thumb_distal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=24, + show_ik_warnings=False, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + FrameTask( + "g1_29dof_rev_1_0_left_wrist_yaw_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + FrameTask( + "g1_29dof_rev_1_0_right_wrist_yaw_link", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "g1_29dof_rev_1_0_left_wrist_yaw_link", + "g1_29dof_rev_1_0_right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], + xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), + ), + enable_gravity_compensation=False, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + + object = ObsTerm( + func=mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_object = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": [-0.01, 0.01], + "y": [-0.01, 0.01], + }, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object"), + }, + ) + + +@configclass +class PickPlaceG1InspireFTPEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the GR1T2 environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events = EventCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + # Temporary directory for URDF files + temp_urdf_dir = tempfile.gettempdir() + + # Idle action to hold robot in default pose + # Action format: [left arm pos (3), left arm quat (4), right arm pos (3), right arm quat (4), + # left hand joint pos (12), right hand joint pos (12)] + idle_action = torch.tensor([ + # 14 hand joints for EEF control + -0.1487, + 0.2038, + 1.0952, + 0.707, + 0.0, + 0.0, + 0.707, + 0.1487, + 0.2038, + 1.0952, + 0.707, + 0.0, + 0.0, + 0.707, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ]) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 6 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 120 # 120Hz + self.sim.render_interval = 2 + + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path + self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + UnitreeG1RetargeterCfg( + enable_visualization=True, + # number of joints in both hands + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + # Please confirm that self.actions.pink_ik_cfg.hand_joint_names is consistent with robot.joint_names[-24:] + # The order of the joints does matter as it will be used for converting pink_ik actions to final control actions in IsaacLab. + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "manusvive": ManusViveCfg( + retargeters=[ + UnitreeG1RetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + }, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/__init__.py new file mode 100644 index 00000000000..d2bbb58b0cb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the place environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/__init__.py new file mode 100644 index 00000000000..d2bbb58b0cb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the place environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/__init__.py new file mode 100644 index 00000000000..6941186bea4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/__init__.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2025, 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 gymnasium as gym + +## +# Register Gym environments. +## + +## +# Agibot Right Arm: place toy2box task, with RmpFlow +## +gym.register( + id="Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.place_toy2box_rmp_rel_env_cfg:RmpFlowAgibotPlaceToy2BoxEnvCfg", + }, + disable_env_checker=True, +) + +## +# Agibot Left Arm: place upright mug task, with RmpFlow +## +gym.register( + id="Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.place_upright_mug_rmp_rel_env_cfg:RmpFlowAgibotPlaceUprightMugEnvCfg", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py new file mode 100644 index 00000000000..18d8ccdf1cb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -0,0 +1,346 @@ +# Copyright (c) 2022-2025, 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 os +from dataclasses import MISSING + +from isaaclab.assets import AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg +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 SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg, RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.place import mdp as place_mdp +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events +from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import ObjectTableSceneCfg + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.agibot import AGIBOT_A2D_CFG # isort: skip +from isaaclab.controllers.config.rmp_flow import AGIBOT_RIGHT_ARM_RMPFLOW_CFG # isort: skip + +## +# Event settings +## + + +@configclass +class EventCfgPlaceToy2Box: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True}) + + init_toy_position = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": { + "x": (-0.15, 0.20), + "y": (-0.3, -0.15), + "z": (-0.65, -0.65), + "yaw": (-3.14, 3.14), + }, + "asset_cfgs": [SceneEntityCfg("toy_truck")], + }, + ) + init_box_position = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": { + "x": (0.25, 0.35), + "y": (0.0, 0.10), + "z": (-0.55, -0.55), + "yaw": (-3.14, 3.14), + }, + "asset_cfgs": [SceneEntityCfg("box")], + }, + ) + + +# +# MDP settings +## + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + toy_truck_positions = ObsTerm( + func=place_mdp.object_poses_in_base_frame, + params={"object_cfg": SceneEntityCfg("toy_truck"), "return_key": "pos"}, + ) + toy_truck_orientations = ObsTerm( + func=place_mdp.object_poses_in_base_frame, + params={"object_cfg": SceneEntityCfg("toy_truck"), "return_key": "quat"}, + ) + box_positions = ObsTerm( + func=place_mdp.object_poses_in_base_frame, params={"object_cfg": SceneEntityCfg("box"), "return_key": "pos"} + ) + box_orientations = ObsTerm( + func=place_mdp.object_poses_in_base_frame, + params={"object_cfg": SceneEntityCfg("box"), "return_key": "quat"}, + ) + eef_pos = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "pos"}) + eef_quat = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "quat"}) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp = ObsTerm( + func=place_mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("toy_truck"), + "diff_threshold": 0.05, + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + toy_truck_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.85, "asset_cfg": SceneEntityCfg("toy_truck")} + ) + + success = DoneTerm( + func=place_mdp.object_a_is_into_b, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "object_a_cfg": SceneEntityCfg("toy_truck"), + "object_b_cfg": SceneEntityCfg("box"), + "xy_threshold": 0.10, + "height_diff": 0.06, + "height_threshold": 0.04, + }, + ) + + +@configclass +class PlaceToy2BoxEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the stacking environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=4096, env_spacing=3.0, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + commands = None + rewards = None + events = None + curriculum = None + + def __post_init__(self): + """Post initialization.""" + + self.sim.render_interval = self.decimation + + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physx.friction_correlation_distance = 0.00625 + + # set viewer to see the whole scene + self.viewer.eye = [1.5, -1.0, 1.5] + self.viewer.lookat = [0.5, 0.0, 0.0] + + +""" +Env to Replay Sim2Lab Demonstrations with JointSpaceAction +""" + + +class RmpFlowAgibotPlaceToy2BoxEnvCfg(PlaceToy2BoxEnvCfg): + + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.events = EventCfgPlaceToy2Box() + + # Set Agibot as robot + self.scene.robot = AGIBOT_A2D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # add table + self.scene.table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0.0, -0.7], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + scale=(1.8, 1.0, 0.30), + ), + ) + + use_relative_mode_env = os.getenv("USE_RELATIVE_MODE", "True") + self.use_relative_mode = use_relative_mode_env.lower() in ["true", "1", "t"] + + # Set actions for the specific robot type (Agibot) + self.actions.arm_action = RMPFlowActionCfg( + asset_name="robot", + joint_names=["right_arm_joint.*"], + body_name="right_gripper_center", + controller=AGIBOT_RIGHT_ARM_RMPFLOW_CFG, + scale=1.0, + body_offset=RMPFlowActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]), + articulation_prim_expr="/World/envs/env_.*/Robot", + use_relative_mode=self.use_relative_mode, + ) + + # Enable Parallel Gripper: + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["right_hand_joint1", "right_.*_Support_Joint"], + open_command_expr={"right_hand_joint1": 0.994, "right_.*_Support_Joint": 0.994}, + close_command_expr={"right_hand_joint1": 0.20, "right_.*_Support_Joint": 0.20}, + ) + + # find joint ids for grippers + self.gripper_joint_names = ["right_hand_joint1", "right_Right_1_Joint"] + self.gripper_open_val = 0.994 + self.gripper_threshold = 0.2 + + # Rigid body properties of toy_truck and box + toy_truck_properties = RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + box_properties = toy_truck_properties.copy() + + # Notes: remember to add Physics/Mass properties to the toy_truck mesh to make grasping successful, + # then you can use below MassPropertiesCfg to set the mass of the toy_truck + toy_mass_properties = MassPropertiesCfg( + mass=0.05, + ) + + self.scene.toy_truck = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/ToyTruck", + init_state=RigidObjectCfg.InitialStateCfg(), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Objects/ToyTruck/toy_truck.usd", + rigid_props=toy_truck_properties, + mass_props=toy_mass_properties, + ), + ) + + self.scene.box = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Box", + init_state=RigidObjectCfg.InitialStateCfg(), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Objects/Box/box.usd", + rigid_props=box_properties, + ), + ) + + # Listens to the required transforms + self.marker_cfg = FRAME_MARKER_CFG.copy() + self.marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + self.marker_cfg.prim_path = "/Visuals/FrameTransformer" + + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=False, + visualizer_cfg=self.marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/right_gripper_center", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + ), + ), + ], + ) + + # add contact force sensor for grasped checking + self.scene.contact_grasp = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/right_.*_Pad_Link", + update_period=0.05, + history_length=6, + debug_vis=True, + filter_prim_paths_expr=["{ENV_REGEX_NS}/ToyTruck"], + ) + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) + + # Set the simulation parameters + self.sim.dt = 1 / 60 + self.sim.render_interval = 6 + + self.decimation = 3 + self.episode_length_s = 30.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py new file mode 100644 index 00000000000..6689a9cb154 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py @@ -0,0 +1,282 @@ +# Copyright (c) 2022-2025, 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 os +from dataclasses import MISSING + +from isaaclab.assets import AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg +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 SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.place import mdp as place_mdp +from isaaclab_tasks.manager_based.manipulation.place.config.agibot import place_toy2box_rmp_rel_env_cfg +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.agibot import AGIBOT_A2D_CFG # isort: skip +from isaaclab.controllers.config.rmp_flow import AGIBOT_LEFT_ARM_RMPFLOW_CFG # isort: skip + +## +# Event settings +## + + +@configclass +class EventCfgPlaceUprightMug: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True}) + + randomize_mug_positions = EventTerm( + func=franka_stack_events.randomize_object_pose, + mode="reset", + params={ + "pose_range": { + "x": (-0.05, 0.2), + "y": (-0.10, 0.10), + "z": (0.75, 0.75), + "roll": (-1.57, -1.57), + "yaw": (-0.57, 0.57), + }, + "asset_cfgs": [SceneEntityCfg("mug")], + }, + ) + + +# +# MDP settings +## + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=mdp.last_action) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + mug_positions = ObsTerm( + func=place_mdp.object_poses_in_base_frame, params={"object_cfg": SceneEntityCfg("mug"), "return_key": "pos"} + ) + mug_orientations = ObsTerm( + func=place_mdp.object_poses_in_base_frame, + params={"object_cfg": SceneEntityCfg("mug"), "return_key": "quat"}, + ) + eef_pos = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "pos"}) + eef_quat = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "quat"}) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp = ObsTerm( + func=place_mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("mug"), + "diff_threshold": 0.05, + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + mug_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.85, "asset_cfg": SceneEntityCfg("mug")} + ) + + success = DoneTerm( + func=place_mdp.object_placed_upright, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "object_cfg": SceneEntityCfg("mug"), + "target_height": 0.6, + }, + ) + + +""" +Env to Place Upright Mug with AgiBot Left Arm using RMPFlow +""" + + +class RmpFlowAgibotPlaceUprightMugEnvCfg(place_toy2box_rmp_rel_env_cfg.PlaceToy2BoxEnvCfg): + + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.events = EventCfgPlaceUprightMug() + + # Set Agibot as robot + self.scene.robot = AGIBOT_A2D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.init_state.pos = (-0.60, 0.0, 0.0) + + # reset obs and termination terms + self.observations = ObservationsCfg() + self.terminations = TerminationsCfg() + + # Table + self.scene.table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.50, 0.0, 0.60], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + scale=(1.0, 1.0, 0.60), + ), + ) + + # add contact force sensor for grasped checking + self.scene.contact_grasp = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/right_.*_Pad_Link", + update_period=0.0, + history_length=6, + debug_vis=True, + filter_prim_paths_expr=["{ENV_REGEX_NS}/Mug"], + ) + + use_relative_mode_env = os.getenv("USE_RELATIVE_MODE", "True") + self.use_relative_mode = use_relative_mode_env.lower() in ["true", "1", "t"] + + # Set actions for the specific robot type (Agibot) + self.actions.arm_action = RMPFlowActionCfg( + asset_name="robot", + joint_names=["left_arm_joint.*"], + body_name="gripper_center", + controller=AGIBOT_LEFT_ARM_RMPFLOW_CFG, + scale=1.0, + body_offset=RMPFlowActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0], rot=[0.7071, 0.0, -0.7071, 0.0]), + articulation_prim_expr="/World/envs/env_.*/Robot", + use_relative_mode=self.use_relative_mode, + ) + + # Enable Parallel Gripper: + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["left_hand_joint1", "left_.*_Support_Joint"], + open_command_expr={"left_hand_joint1": 0.994, "left_.*_Support_Joint": 0.994}, + close_command_expr={"left_hand_joint1": 0.0, "left_.*_Support_Joint": 0.0}, + ) + + # find joint ids for grippers + self.gripper_joint_names = ["left_hand_joint1", "left_Right_1_Joint"] + self.gripper_open_val = 0.994 + self.gripper_threshold = 0.2 + + # Rigid body properties of mug + mug_properties = RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + self.scene.mug = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Mug", + init_state=RigidObjectCfg.InitialStateCfg(), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Objects/Mug/mug.usd", + scale=(1.0, 1.0, 1.0), + rigid_props=mug_properties, + ), + ) + + # Listens to the required transforms + self.marker_cfg = FRAME_MARKER_CFG.copy() + self.marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + self.marker_cfg.prim_path = "/Visuals/FrameTransformer" + + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + debug_vis=False, + visualizer_cfg=self.marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/gripper_center", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.0], + rot=[ + 0.7071, + 0.0, + -0.7071, + 0.0, + ], + ), + ), + ], + ) + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + } + ) + + # Set the simulation parameters + self.sim.dt = 1 / 60 + self.sim.render_interval = 6 + + self.decimation = 3 + self.episode_length_s = 10.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/__init__.py new file mode 100644 index 00000000000..f394d204c70 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the pick and place environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/observations.py new file mode 100644 index 00000000000..18870db2cad --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/observations.py @@ -0,0 +1,118 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING, Literal + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformer + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_poses_in_base_frame( + env: ManagerBasedRLEnv, + object_cfg: SceneEntityCfg = SceneEntityCfg("mug"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + return_key: Literal["pos", "quat", None] = None, +) -> torch.Tensor: + """The pose of the object in the robot base frame.""" + object: RigidObject = env.scene[object_cfg.name] + + pos_object_world = object.data.root_pos_w + quat_object_world = object.data.root_quat_w + + """The position of the robot in the world frame.""" + robot: Articulation = env.scene[robot_cfg.name] + root_pos_w = robot.data.root_pos_w + root_quat_w = robot.data.root_quat_w + + pos_object_base, quat_object_base = math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_object_world, quat_object_world + ) + if return_key == "pos": + return pos_object_base + elif return_key == "quat": + return quat_object_base + elif return_key is None: + return torch.cat((pos_object_base, quat_object_base), dim=1) + + +def object_grasped( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + ee_frame_cfg: SceneEntityCfg, + object_cfg: SceneEntityCfg, + diff_threshold: float = 0.06, + force_threshold: float = 1.0, +) -> torch.Tensor: + """ + Check if an object is grasped by the specified robot. + Support both surface gripper and parallel gripper. + If contact_grasp sensor is found, check if the contact force is greater than force_threshold. + """ + + robot: Articulation = env.scene[robot_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + + object_pos = object.data.root_pos_w + end_effector_pos = ee_frame.data.target_pos_w[:, 0, :] + pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1) + + if "contact_grasp" in env.scene.keys() and env.scene["contact_grasp"] is not None: + contact_force_grasp = env.scene["contact_grasp"].data.net_forces_w # shape:(N, 2, 3) for two fingers + contact_force_norm = torch.linalg.vector_norm( + contact_force_grasp, dim=2 + ) # shape:(N, 2) - force magnitude per finger + both_fingers_force_ok = torch.all( + contact_force_norm > force_threshold, dim=1 + ) # both fingers must exceed threshold + grasped = torch.logical_and(pose_diff < diff_threshold, both_fingers_force_ok) + elif ( + f"contact_grasp_{object_cfg.name}" in env.scene.keys() + and env.scene[f"contact_grasp_{object_cfg.name}"] is not None + ): + contact_force_object = env.scene[ + f"contact_grasp_{object_cfg.name}" + ].data.net_forces_w # shape:(N, 2, 3) for two fingers + contact_force_norm = torch.linalg.vector_norm( + contact_force_object, dim=2 + ) # shape:(N, 2) - force magnitude per finger + both_fingers_force_ok = torch.all( + contact_force_norm > force_threshold, dim=1 + ) # both fingers must exceed threshold + grasped = torch.logical_and(pose_diff < diff_threshold, both_fingers_force_ok) + else: + grasped = (pose_diff < diff_threshold).clone().detach() + + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open + suction_cup_is_closed = (suction_cup_status == 1).to(torch.float32) + grasped = torch.logical_and(suction_cup_is_closed, pose_diff < diff_threshold) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + grasped = torch.logical_and( + grasped, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val) + > env.cfg.gripper_threshold, + ) + grasped = torch.logical_and( + grasped, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val) + > env.cfg.gripper_threshold, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return grasped diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/terminations.py new file mode 100644 index 00000000000..9768321ef13 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/mdp/terminations.py @@ -0,0 +1,122 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the place task. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_placed_upright( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + object_cfg: SceneEntityCfg, + target_height: float = 0.927, + euler_xy_threshold: float = 0.10, +): + """Check if an object placed upright by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + + # Compute mug euler angles of X, Y axis, to check if it is placed upright + object_euler_x, object_euler_y, _ = math_utils.euler_xyz_from_quat(object.data.root_quat_w) # (N,4) [0, 2*pi] + + object_euler_x_err = torch.abs(math_utils.wrap_to_pi(object_euler_x)) # (N,) + object_euler_y_err = torch.abs(math_utils.wrap_to_pi(object_euler_y)) # (N,) + + success = torch.logical_and(object_euler_x_err < euler_xy_threshold, object_euler_y_err < euler_xy_threshold) + + # Check if current mug height is greater than target height + height_success = object.data.root_pos_w[:, 2] > target_height + + success = torch.logical_and(height_success, success) + + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open + suction_cup_is_open = (suction_cup_status == -1).to(torch.float32) + success = torch.logical_and(suction_cup_is_open, success) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + success = torch.logical_and( + success, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val) + < env.cfg.gripper_threshold, + ) + success = torch.logical_and( + success, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val) + < env.cfg.gripper_threshold, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return success + + +def object_a_is_into_b( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_a_cfg: SceneEntityCfg = SceneEntityCfg("object_a"), + object_b_cfg: SceneEntityCfg = SceneEntityCfg("object_b"), + xy_threshold: float = 0.03, # xy_distance_threshold + height_threshold: float = 0.04, # height_distance_threshold + height_diff: float = 0.0, # expected height_diff +) -> torch.Tensor: + """Check if an object a is put into another object b by the specified robot.""" + + robot: Articulation = env.scene[robot_cfg.name] + object_a: RigidObject = env.scene[object_a_cfg.name] + object_b: RigidObject = env.scene[object_b_cfg.name] + + # check object a is into object b + pos_diff = object_a.data.root_pos_w - object_b.data.root_pos_w + height_dist = torch.linalg.vector_norm(pos_diff[:, 2:], dim=1) + xy_dist = torch.linalg.vector_norm(pos_diff[:, :2], dim=1) + + success = torch.logical_and(xy_dist < xy_threshold, (height_dist - height_diff) < height_threshold) + + # Check gripper positions + if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0: + surface_gripper = env.scene.surface_grippers["surface_gripper"] + suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open + suction_cup_is_open = (suction_cup_status == -1).to(torch.float32) + success = torch.logical_and(suction_cup_is_open, success) + + else: + if hasattr(env.cfg, "gripper_joint_names"): + gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names) + assert len(gripper_joint_ids) == 2, "Terminations only support parallel gripper for now" + + success = torch.logical_and( + success, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val) + < env.cfg.gripper_threshold, + ) + success = torch.logical_and( + success, + torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val) + < env.cfg.gripper_threshold, + ) + else: + raise ValueError("No gripper_joint_names found in environment config") + + return success diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml index 62cef0dde2d..d6cf3c8dd25 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml index f6412089ff0..f14c8a6094b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/skrl_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [64, 64] activations: elu output: ONE diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_joint_pos_env_cfg.py index fbc6454bba8..2952593df86 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/bin_stack_joint_pos_env_cfg.py @@ -113,6 +113,10 @@ def __post_init__(self): open_command_expr={"panda_finger_.*": 0.04}, close_command_expr={"panda_finger_.*": 0.0}, ) + # utilities for gripper status check + self.gripper_joint_names = ["panda_finger_.*"] + self.gripper_open_val = 0.04 + self.gripper_threshold = 0.005 # Rigid body properties of each cube cube_properties = RigidBodyPropertiesCfg( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_cosmos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_cosmos_env_cfg.py index 6dda2c8b427..e5b181abaef 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_cosmos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_visuomotor_cosmos_env_cfg.py @@ -105,11 +105,8 @@ def __post_init__(self): # post init of parent super().__post_init__() - import carb - from isaacsim.core.utils.carb import set_carb_setting - - carb_setting = carb.settings.get_settings() - set_carb_setting(carb_setting, "/rtx/domeLight/upperLowerStrategy", 4) + # set domeLight.upperLowerStrategy to 4 to remove rendering noise + self.sim.render.dome_light_upper_lower_strategy = 4 SEMANTIC_MAPPING = { "class:cube_1": (120, 230, 255, 255), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py index cc91754363d..ae01d277ba5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py @@ -87,6 +87,7 @@ def __post_init__(self): open_command_expr={"panda_finger_.*": 0.04}, close_command_expr={"panda_finger_.*": 0.0}, ) + # utilities for gripper status check self.gripper_joint_names = ["panda_finger_.*"] self.gripper_open_val = 0.04 self.gripper_threshold = 0.005 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py index 51e2ecb8cc8..5ac1e9e2d2b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_instance_randomize_env_cfg.py @@ -87,6 +87,10 @@ def __post_init__(self): open_command_expr={"panda_finger_.*": 0.04}, close_command_expr={"panda_finger_.*": 0.0}, ) + # utilities for gripper status check + self.gripper_joint_names = ["panda_finger_.*"] + self.gripper_open_val = 0.04 + self.gripper_threshold = 0.005 # Rigid body properties of each cube cube_properties = RigidBodyPropertiesCfg( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/__init__.py index 06bc8dcbf06..7aa2ebad0fc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/__init__.py @@ -5,9 +5,6 @@ import gymnasium as gym -import os - -from . import stack_rmp_rel_env_cfg ## # Register Gym environments. @@ -21,7 +18,7 @@ id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_rmp_rel_env_cfg.RmpFlowGalbotLeftArmCubeStackEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotLeftArmCubeStackEnvCfg", }, disable_env_checker=True, ) @@ -31,7 +28,7 @@ id="Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_rmp_rel_env_cfg.RmpFlowGalbotRightArmCubeStackEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotRightArmCubeStackEnvCfg", }, disable_env_checker=True, ) @@ -44,7 +41,7 @@ id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_rmp_rel_env_cfg.RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:RmpFlowGalbotLeftArmCubeStackVisuomotorEnvCfg", }, disable_env_checker=True, ) @@ -56,7 +53,9 @@ id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Joint-Position-Play-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_rmp_rel_env_cfg.GalbotLeftArmJointPositionCubeStackVisuomotorEnvCfg_PLAY, + "env_cfg_entry_point": ( + f"{__name__}.stack_rmp_rel_env_cfg:GalbotLeftArmJointPositionCubeStackVisuomotorEnvCfg_PLAY" + ), }, disable_env_checker=True, ) @@ -68,7 +67,7 @@ id="Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-RmpFlow-Play-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_rmp_rel_env_cfg.GalbotLeftArmRmpFlowCubeStackVisuomotorEnvCfg_PLAY, + "env_cfg_entry_point": f"{__name__}.stack_rmp_rel_env_cfg:GalbotLeftArmRmpFlowCubeStackVisuomotorEnvCfg_PLAY", }, disable_env_checker=True, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py index af7c2c07c4a..cdf9baeb4e5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py @@ -5,6 +5,9 @@ from isaaclab.assets import RigidObjectCfg, SurfaceGripperCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.openxr import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GripperRetargeterCfg, Se3AbsRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import SurfaceGripperBinaryActionCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup @@ -12,7 +15,7 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import FrameTransformerCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg -from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.schemas.schemas_cfg import CollisionPropertiesCfg, RigidBodyPropertiesCfg from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -190,6 +193,7 @@ def __post_init__(self): max_depenetration_velocity=5.0, disable_gravity=False, ) + cube_collision_properties = CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0.0) # Set each stacking cube deterministically self.scene.cube_1 = RigidObjectCfg( @@ -199,6 +203,7 @@ def __post_init__(self): usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", scale=(1.0, 1.0, 1.0), rigid_props=cube_properties, + collision_props=cube_collision_properties, ), ) self.scene.cube_2 = RigidObjectCfg( @@ -208,6 +213,7 @@ def __post_init__(self): usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", scale=(1.0, 1.0, 1.0), rigid_props=cube_properties, + collision_props=cube_collision_properties, ), ) self.scene.cube_3 = RigidObjectCfg( @@ -217,6 +223,7 @@ def __post_init__(self): usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd", scale=(1.0, 1.0, 1.0), rigid_props=cube_properties, + collision_props=cube_collision_properties, ), ) @@ -240,6 +247,27 @@ def __post_init__(self): ], ) + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + @configclass class GalbotRightArmCubeStackEnvCfg(GalbotLeftArmCubeStackEnvCfg): @@ -262,7 +290,7 @@ def __post_init__(self): # Set surface gripper: Ensure the SurfaceGripper prim has the required attributes self.scene.surface_gripper = SurfaceGripperCfg( prim_path="{ENV_REGEX_NS}/Robot/right_suction_cup_tcp_link/SurfaceGripper", - max_grip_distance=0.02, + max_grip_distance=0.0075, shear_force_limit=5000.0, coaxial_force_limit=5000.0, retry_interval=0.05, @@ -276,3 +304,24 @@ def __post_init__(self): ) self.scene.ee_frame.target_frames[0].prim_path = "{ENV_REGEX_NS}/Robot/right_suction_cup_tcp_link" + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py index 7aafc6990f3..8eb970dc3e5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py @@ -9,6 +9,8 @@ import isaaclab.sim as sim_utils from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.openxr import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import GripperRetargeterCfg, Se3RelRetargeterCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg from isaaclab.sensors import CameraCfg, FrameTransformerCfg @@ -75,6 +77,24 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + delta_pos_scale_factor=10.0, + delta_rot_scale_factor=10.0, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), } ) @@ -106,12 +126,15 @@ def __post_init__(self): use_relative_mode=self.use_relative_mode, ) # Set the simulation parameters - self.sim.dt = 1 / 60 - self.sim.render_interval = 1 + self.sim.dt = 1 / 120 + self.sim.render_interval = 6 - self.decimation = 3 + self.decimation = 6 self.episode_length_s = 30.0 + # Enable CCD to avoid tunneling + self.sim.physx.enable_ccd = True + self.teleop_devices = DevicesCfg( devices={ "keyboard": Se3KeyboardCfg( @@ -124,6 +147,24 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + delta_pos_scale_factor=10.0, + delta_rot_scale_factor=10.0, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), } ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/__init__.py index d051b5fc548..41887a8df8b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/__init__.py @@ -5,8 +5,6 @@ import gymnasium as gym -from . import stack_ik_rel_env_cfg - ## # Register Gym environments. ## @@ -20,7 +18,7 @@ id="Isaac-Stack-Cube-UR10-Long-Suction-IK-Rel-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_ik_rel_env_cfg.UR10LongSuctionCubeStackEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_env_cfg:UR10LongSuctionCubeStackEnvCfg", }, disable_env_checker=True, ) @@ -29,7 +27,7 @@ id="Isaac-Stack-Cube-UR10-Short-Suction-IK-Rel-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", kwargs={ - "env_cfg_entry_point": stack_ik_rel_env_cfg.UR10ShortSuctionCubeStackEnvCfg, + "env_cfg_entry_point": f"{__name__}.stack_ik_rel_env_cfg:UR10ShortSuctionCubeStackEnvCfg", }, disable_env_checker=True, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py index 467df1d4410..726d9079472 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_joint_pos_env_cfg.py @@ -152,7 +152,7 @@ def __post_init__(self): # Set surface gripper: Ensure the SurfaceGripper prim has the required attributes self.scene.surface_gripper = SurfaceGripperCfg( prim_path="{ENV_REGEX_NS}/Robot/ee_link/SurfaceGripper", - max_grip_distance=0.05, + max_grip_distance=0.0075, shear_force_limit=5000.0, coaxial_force_limit=5000.0, retry_interval=0.05, @@ -190,7 +190,7 @@ def __post_init__(self): # Set surface gripper: Ensure the SurfaceGripper prim has the required attributes self.scene.surface_gripper = SurfaceGripperCfg( prim_path="{ENV_REGEX_NS}/Robot/ee_link/SurfaceGripper", - max_grip_distance=0.05, + max_grip_distance=0.0075, shear_force_limit=5000.0, coaxial_force_limit=5000.0, retry_interval=0.05, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml index 005f95806d1..5473188cbd8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/skrl_flat_ppo_cfg.yaml @@ -19,7 +19,7 @@ models: initial_log_std: -0.6931471805599453 network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128] activations: elu output: ACTIONS @@ -28,7 +28,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [128, 128] activations: elu output: ONE diff --git a/source/isaaclab_tasks/setup.py b/source/isaaclab_tasks/setup.py index 34001574112..38a1d1a6e02 100644 --- a/source/isaaclab_tasks/setup.py +++ b/source/isaaclab_tasks/setup.py @@ -24,8 +24,6 @@ "protobuf>=4.25.8,!=5.26.0", # basic logger "tensorboard", - # automate - "scikit-learn", "numba", ] @@ -51,6 +49,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py index 23a92bab9c1..1034fd9ac92 100644 --- a/source/isaaclab_tasks/test/env_test_utils.py +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -123,11 +123,13 @@ def _run_environments( "Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0", "Isaac-Stack-Cube-Instance-Randomize-Franka-IK-Rel-v0", "Isaac-Stack-Cube-Instance-Randomize-Franka-v0", - "Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0", - "Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos-v0", ]: return + # skip these environments as they cannot be run with 32 environments within reasonable VRAM + if "Visuomotor" in task_name and num_envs == 32: + return + # skip automate environments as they require cuda installation if task_name in ["Isaac-AutoMate-Assembly-Direct-v0", "Isaac-AutoMate-Disassembly-Direct-v0"]: return diff --git a/texture/textureNAO.png b/texture/textureNAO.png new file mode 100644 index 00000000000..4c4dcfbb1d7 Binary files /dev/null and b/texture/textureNAO.png differ diff --git a/tools/template/templates/agents/sb3_ppo_cfg b/tools/template/templates/agents/sb3_ppo_cfg index 5856f35f8e8..4ac83212e44 100644 --- a/tools/template/templates/agents/sb3_ppo_cfg +++ b/tools/template/templates/agents/sb3_ppo_cfg @@ -11,11 +11,10 @@ n_epochs: 20 ent_coef: 0.01 learning_rate: !!float 3e-4 clip_range: !!float 0.2 -policy_kwargs: "dict( - activation_fn=nn.ELU, - net_arch=[32, 32], - squash_output=False, - )" +policy_kwargs: + activation_fn: nn.ELU + net_arch: [32, 32] + squash_output: False vf_coef: 1.0 max_grad_norm: 1.0 device: "cuda:0" diff --git a/tools/template/templates/agents/skrl_amp_cfg b/tools/template/templates/agents/skrl_amp_cfg index e435b44eac9..0946e4c6e6f 100644 --- a/tools/template/templates/agents/skrl_amp_cfg +++ b/tools/template/templates/agents/skrl_amp_cfg @@ -15,7 +15,7 @@ models: fixed_log_std: True network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ACTIONS @@ -24,7 +24,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE @@ -33,7 +33,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [1024, 512] activations: relu output: ONE diff --git a/tools/template/templates/agents/skrl_ippo_cfg b/tools/template/templates/agents/skrl_ippo_cfg index bc0c5182179..a89939f9554 100644 --- a/tools/template/templates/agents/skrl_ippo_cfg +++ b/tools/template/templates/agents/skrl_ippo_cfg @@ -14,7 +14,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,7 +23,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/tools/template/templates/agents/skrl_mappo_cfg b/tools/template/templates/agents/skrl_mappo_cfg index dcd794f57a5..255b30eac81 100644 --- a/tools/template/templates/agents/skrl_mappo_cfg +++ b/tools/template/templates/agents/skrl_mappo_cfg @@ -14,7 +14,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,7 +23,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/tools/template/templates/agents/skrl_ppo_cfg b/tools/template/templates/agents/skrl_ppo_cfg index 1efe67083a5..96515145fab 100644 --- a/tools/template/templates/agents/skrl_ppo_cfg +++ b/tools/template/templates/agents/skrl_ppo_cfg @@ -14,7 +14,7 @@ models: initial_log_std: 0.0 network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ACTIONS @@ -23,7 +23,7 @@ models: clip_actions: False network: - name: net - input: STATES + input: OBSERVATIONS layers: [32, 32] activations: elu output: ONE diff --git a/tools/template/templates/extension/setup.py b/tools/template/templates/extension/setup.py index 55f278b5b87..c4c68f4b056 100644 --- a/tools/template/templates/extension/setup.py +++ b/tools/template/templates/extension/setup.py @@ -41,6 +41,7 @@ "Programming Language :: Python :: 3.11", "Isaac Sim :: 4.5.0", "Isaac Sim :: 5.0.0", + "Isaac Sim :: 5.1.0", ], zip_safe=False, ) diff --git a/tools/test_settings.py b/tools/test_settings.py index 936d38c4958..7ba3f6e3def 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -18,17 +18,22 @@ PER_TEST_TIMEOUTS = { "test_articulation.py": 500, "test_stage_in_memory.py": 500, - "test_environments.py": 2000, # This test runs through all the environments for 100 steps each + "test_environments.py": 2500, # This test runs through all the environments for 100 steps each "test_environments_with_stage_in_memory.py": ( - 2000 + 2500 ), # Like the above, with stage in memory and with and without fabric cloning - "test_environment_determinism.py": 500, # This test runs through many the environments for 100 steps each + "test_environment_determinism.py": 1000, # This test runs through many the environments for 100 steps each "test_factory_environments.py": 1000, # This test runs through Factory environments for 100 steps each "test_multi_agent_environments.py": 800, # This test runs through multi-agent environments for 100 steps each "test_generate_dataset.py": 500, # This test runs annotation for 10 demos and generation until one succeeds - "test_environments_training.py": 6000, + "test_pink_ik.py": 1000, # This test runs through all the pink IK environments through various motions + "test_environments_training.py": ( + 6000 + ), # This test runs through training for several environments and compares thresholds "test_simulation_render_config.py": 500, "test_operational_space.py": 500, + "test_non_headless_launch.py": 1000, # This test launches the app in non-headless mode and starts simulation + "test_rl_games_wrapper.py": 500, } """A dictionary of tests and their timeouts in seconds. diff --git a/uninstall b/uninstall new file mode 100755 index 00000000000..659b98c24c5 Binary files /dev/null and b/uninstall differ diff --git a/uninstall.dat b/uninstall.dat new file mode 100644 index 00000000000..7b3f11a2dd5 Binary files /dev/null and b/uninstall.dat differ